ikfast_lwr.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 #include <cmath>
00021 #include <vector>
00022 #include <limits>
00023 #include <algorithm>
00024 #include <complex>
00025 
00026 #ifdef IKFAST_HEADER
00027 #include IKFAST_HEADER
00028 #endif
00029 
00030 #ifndef IKFAST_ASSERT
00031 #include <stdexcept>
00032 #include <sstream>
00033 
00034 #ifdef _MSC_VER
00035 #ifndef __PRETTY_FUNCTION__
00036 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00037 #endif
00038 #endif
00039 
00040 #ifndef __PRETTY_FUNCTION__
00041 #define __PRETTY_FUNCTION__ __func__
00042 #endif
00043 
00044 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
00045 
00046 #endif
00047 
00048 #if defined(_MSC_VER)
00049 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
00050 #else
00051 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
00052 #endif
00053 
00054 #define IK2PI  ((IKReal)6.28318530717959)
00055 #define IKPI  ((IKReal)3.14159265358979)
00056 #define IKPI_2  ((IKReal)1.57079632679490)
00057 
00058 #ifdef _MSC_VER
00059 #ifndef isnan
00060 #define isnan _isnan
00061 #endif
00062 #endif // _MSC_VER
00063 
00064 // defined when creating a shared object/dll
00065 #ifdef IKFAST_CLIBRARY
00066 #ifdef _MSC_VER
00067 #define IKFAST_API extern "C" __declspec(dllexport)
00068 #else
00069 #define IKFAST_API extern "C"
00070 #endif
00071 #else
00072 #define IKFAST_API
00073 #endif
00074 
00075 // lapack routines
00076 extern "C" {
00077   void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
00078   void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
00079   void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
00080   void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
00081   void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
00082   void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
00083 }
00084 
00085 using namespace std; // necessary to get std math routines
00086 
00087 #ifdef IKFAST_NAMESPACE
00088 namespace IKFAST_NAMESPACE {
00089 #endif
00090 
00091 #ifdef IKFAST_REAL
00092 typedef IKFAST_REAL IKReal;
00093 #else
00094 typedef double IKReal;
00095 #endif
00096 
00097 class IKSolution
00098 {
00099 public:
00102     void GetSolution(IKReal* psolution, const IKReal* pfree) const {
00103         for(std::size_t i = 0; i < basesol.size(); ++i) {
00104             if( basesol[i].freeind < 0 )
00105                 psolution[i] = basesol[i].foffset;
00106             else {
00107                 IKFAST_ASSERT(pfree != NULL);
00108                 psolution[i] = pfree[basesol[i].freeind]*basesol[i].fmul + basesol[i].foffset;
00109                 if( psolution[i] > IKPI ) {
00110                     psolution[i] -= IK2PI;
00111                 }
00112                 else if( psolution[i] < -IKPI ) {
00113                     psolution[i] += IK2PI;
00114                 }
00115             }
00116         }
00117     }
00118 
00121     const std::vector<int>& GetFree() const { return vfree; }
00122 
00123     struct VARIABLE
00124     {
00125         VARIABLE() : freeind(-1), fmul(0), foffset(0) {}
00126         VARIABLE(int freeind, IKReal fmul, IKReal foffset) : freeind(freeind), fmul(fmul), foffset(foffset) {}
00127         int freeind;
00128         IKReal fmul, foffset; 
00129     };
00130 
00131     std::vector<VARIABLE> basesol;       
00132     std::vector<int> vfree;
00133 };
00134 
00135 inline float IKabs(float f) { return fabsf(f); }
00136 inline double IKabs(double f) { return fabs(f); }
00137 
00138 inline float IKlog(float f) { return logf(f); }
00139 inline double IKlog(double f) { return log(f); }
00140 
00141 #ifndef IKFAST_SINCOS_THRESH
00142 #define IKFAST_SINCOS_THRESH ((IKReal)0.000001)
00143 #endif
00144 
00145 inline float IKasin(float f)
00146 {
00147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00148 if( f <= -1 ) return -IKPI_2;
00149 else if( f >= 1 ) return IKPI_2;
00150 return asinf(f);
00151 }
00152 inline double IKasin(double f)
00153 {
00154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00155 if( f <= -1 ) return -IKPI_2;
00156 else if( f >= 1 ) return IKPI_2;
00157 return asin(f);
00158 }
00159 
00160 // return positive value in [0,y)
00161 inline float IKfmod(float x, float y)
00162 {
00163     while(x < 0) {
00164         x += y;
00165     }
00166     return fmodf(x,y);
00167 }
00168 
00169 // return positive value in [0,y)
00170 inline float IKfmod(double x, double y)
00171 {
00172     while(x < 0) {
00173         x += y;
00174     }
00175     return fmod(x,y);
00176 }
00177 
00178 inline float IKacos(float f)
00179 {
00180 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00181 if( f <= -1 ) return IKPI;
00182 else if( f >= 1 ) return 0;
00183 return acosf(f);
00184 }
00185 inline double IKacos(double f)
00186 {
00187 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
00188 if( f <= -1 ) return IKPI;
00189 else if( f >= 1 ) return 0;
00190 return acos(f);
00191 }
00192 inline float IKsin(float f) { return sinf(f); }
00193 inline double IKsin(double f) { return sin(f); }
00194 inline float IKcos(float f) { return cosf(f); }
00195 inline double IKcos(double f) { return cos(f); }
00196 inline float IKtan(float f) { return tanf(f); }
00197 inline double IKtan(double f) { return tan(f); }
00198 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
00199 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
00200 inline float IKatan2(float fy, float fx) {
00201     if( isnan(fy) ) {
00202         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00203         return IKPI_2;
00204     }
00205     else if( isnan(fx) ) {
00206         return 0;
00207     }
00208     return atan2f(fy,fx);
00209 }
00210 inline double IKatan2(double fy, double fx) {
00211     if( isnan(fy) ) {
00212         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
00213         return IKPI_2;
00214     }
00215     else if( isnan(fx) ) {
00216         return 0;
00217     }
00218     return atan2(fy,fx);
00219 }
00220 
00221 inline float IKsign(float f) {
00222     if( f > 0 ) {
00223         return 1.0f;
00224     }
00225     else if( f < 0 ) {
00226         return -1.0f;
00227     }
00228     return 0;
00229 }
00230 
00231 inline double IKsign(double f) {
00232     if( f > 0 ) {
00233         return 1.0;
00234     }
00235     else if( f < 0 ) {
00236         return -1.0;
00237     }
00238     return 0;
00239 }
00240 
00243 IKFAST_API void fk(const IKReal* j, IKReal* eetrans, IKReal* eerot) {
00244 IKReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63,x64,x65,x66;
00245 x0=IKcos(j[0]);
00246 x1=IKcos(j[1]);
00247 x2=IKcos(j[2]);
00248 x3=IKsin(j[0]);
00249 x4=IKsin(j[2]);
00250 x5=IKsin(j[3]);
00251 x6=((x0)*(x1)*(x2));
00252 x7=((x3)*(x4));
00253 x8=((x6)+(((-1.00000000000000)*(x7))));
00254 x9=IKcos(j[3]);
00255 x10=IKsin(j[1]);
00256 x11=IKsin(j[4]);
00257 x12=((x2)*(x3));
00258 x13=((x0)*(x1)*(x4));
00259 x14=((x13)+(x12));
00260 x15=((-1.00000000000000)*(x14));
00261 x16=IKcos(j[4]);
00262 x17=IKcos(j[6]);
00263 x18=((x15)*(x16));
00264 x19=((x8)*(x9));
00265 x20=((x0)*(x10)*(x5));
00266 x21=((x19)+(x20));
00267 x22=IKsin(j[6]);
00268 x23=IKcos(j[5]);
00269 x24=((x11)*(x15));
00270 x25=((x16)*(x21));
00271 x26=((x24)+(x25));
00272 x27=IKsin(j[5]);
00273 x28=((x5)*(x8));
00274 x29=((-1.00000000000000)*(x0)*(x10)*(x9));
00275 x30=((x28)+(x29));
00276 x31=((x0)*(x4));
00277 x32=((x1)*(x12));
00278 x33=((x32)+(x31));
00279 x34=((x0)*(x2));
00280 x35=((x1)*(x7));
00281 x36=((((-1.00000000000000)*(x35)))+(x34));
00282 x37=((x33)*(x9));
00283 x38=((x10)*(x3)*(x5));
00284 x39=((x38)+(x37));
00285 x40=((x33)*(x5));
00286 x41=((x10)*(x3)*(x9));
00287 x42=((((-1.00000000000000)*(x41)))+(x40));
00288 x43=((x11)*(x36));
00289 x44=((x16)*(x39));
00290 x45=((x44)+(x43));
00291 x46=((x16)*(x36));
00292 x47=((-1.00000000000000)*(x39));
00293 x48=((x11)*(x47));
00294 x49=((x48)+(x46));
00295 x50=((x10)*(x2)*(x9));
00296 x51=((x1)*(x5));
00297 x52=((x50)+(((-1.00000000000000)*(x51))));
00298 x53=((x11)*(x52));
00299 x54=((x10)*(x16)*(x4));
00300 x55=((x54)+(x53));
00301 x56=((x10)*(x11)*(x4));
00302 x57=((x51)+(((-1.00000000000000)*(x50))));
00303 x58=((x16)*(x57));
00304 x59=((x58)+(x56));
00305 x60=((x23)*(x59));
00306 x61=((x10)*(x2)*(x5));
00307 x62=((x1)*(x9));
00308 x63=((x61)+(x62));
00309 x64=((-1.00000000000000)*(x63));
00310 x65=((x27)*(x64));
00311 x66=((x60)+(x65));
00312 eerot[0]=((((x17)*(((((x27)*(x30)))+(((x23)*(x26)))))))+(((x22)*(((((x11)*(((((-1.00000000000000)*(x19)))+(((-1.00000000000000)*(x20)))))))+(x18))))));
00313 eerot[1]=((((x22)*(((((-1.00000000000000)*(x23)*(x26)))+(((-1.00000000000000)*(x27)*(x30)))))))+(((x17)*(((x18)+(((-1.00000000000000)*(x11)*(x21))))))));
00314 eerot[2]=((((x23)*(((((-1.00000000000000)*(x28)))+(((x0)*(x10)*(x9)))))))+(((x26)*(x27))));
00315 eetrans[0]=((((x23)*(((((-0.0780000000000000)*(x28)))+(((0.0780000000000000)*(x0)*(x10)*(x9)))))))+(((0.400000000000000)*(x0)*(x10)))+(((x27)*(((((0.0780000000000000)*(x25)))+(((0.0780000000000000)*(x24)))))))+(((x5)*(((((0.390000000000000)*(x7)))+(((-0.390000000000000)*(x6)))))))+(((0.390000000000000)*(x0)*(x10)*(x9))));
00316 eerot[3]=((((x17)*(((((x27)*(x42)))+(((x23)*(x45)))))))+(((x22)*(x49))));
00317 eerot[4]=((((x22)*(((((-1.00000000000000)*(x23)*(x45)))+(((-1.00000000000000)*(x27)*(x42)))))))+(((x17)*(x49))));
00318 eerot[5]=((((x23)*(((((-1.00000000000000)*(x40)))+(x41)))))+(((x27)*(x45))));
00319 eetrans[1]=((((x23)*(((((0.0780000000000000)*(x41)))+(((-0.0780000000000000)*(x40)))))))+(((x5)*(((((-0.390000000000000)*(x32)))+(((-0.390000000000000)*(x31)))))))+(((0.390000000000000)*(x41)))+(((0.400000000000000)*(x10)*(x3)))+(((x27)*(((((0.0780000000000000)*(x44)))+(((0.0780000000000000)*(x43))))))));
00320 eerot[6]=((((x17)*(x66)))+(((x22)*(x55))));
00321 eerot[7]=((((x17)*(x55)))+(((-1.00000000000000)*(x22)*(x66))));
00322 eerot[8]=((((x23)*(x63)))+(((x27)*(x59))));
00323 eetrans[2]=((0.3105)+(((0.400000000000000)*(x1)))+(((0.390000000000000)*(x61)))+(((0.390000000000000)*(x62)))+(((x27)*(((((0.0780000000000000)*(x58)))+(((0.0780000000000000)*(x56)))))))+(((x23)*(((((0.0780000000000000)*(x62)))+(((0.0780000000000000)*(x61))))))));
00324 }
00325 
00326 IKFAST_API int getNumFreeParameters() { return 1; }
00327 IKFAST_API int* getFreeParameters() { static int freeparams[] = {2}; return freeparams; }
00328 IKFAST_API int getNumJoints() { return 7; }
00329 
00330 IKFAST_API int getIKRealSize() { return sizeof(IKReal); }
00331 
00332 IKFAST_API int getIKType() { return 0x67000001; }
00333 
00334 class IKSolver {
00335 public:
00336 IKReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j2,cj2,sj2,htj2,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
00337 
00338 bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
00339 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00340 vsolutions.resize(0); vsolutions.reserve(8);
00341 j2=pfree[0]; cj2=cos(pfree[0]); sj2=sin(pfree[0]);
00342 r00 = eerot[0*3+0];
00343 r01 = eerot[0*3+1];
00344 r02 = eerot[0*3+2];
00345 r10 = eerot[1*3+0];
00346 r11 = eerot[1*3+1];
00347 r12 = eerot[1*3+2];
00348 r20 = eerot[2*3+0];
00349 r21 = eerot[2*3+1];
00350 r22 = eerot[2*3+2];
00351 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00352 
00353 new_r00=r00;
00354 new_r01=r01;
00355 new_r02=r02;
00356 new_px=((((-0.0780000000000000)*(r02)))+(px));
00357 new_r10=r10;
00358 new_r11=r11;
00359 new_r12=r12;
00360 new_py=((((-0.0780000000000000)*(r12)))+(py));
00361 new_r20=r20;
00362 new_r21=r21;
00363 new_r22=r22;
00364 new_pz=((-0.3105)+(((-0.0780000000000000)*(r22)))+(pz));
00365 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
00366 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00367 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00368 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00369 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00370 rxp0_0=((((-1.00000000000000)*(py)*(r20)))+(((pz)*(r10))));
00371 rxp0_1=((((px)*(r20)))+(((-1.00000000000000)*(pz)*(r00))));
00372 rxp0_2=((((-1.00000000000000)*(px)*(r10)))+(((py)*(r00))));
00373 rxp1_0=((((-1.00000000000000)*(py)*(r21)))+(((pz)*(r11))));
00374 rxp1_1=((((px)*(r21)))+(((-1.00000000000000)*(pz)*(r01))));
00375 rxp1_2=((((-1.00000000000000)*(px)*(r11)))+(((py)*(r01))));
00376 rxp2_0=((((-1.00000000000000)*(py)*(r22)))+(((pz)*(r12))));
00377 rxp2_1=((((px)*(r22)))+(((-1.00000000000000)*(pz)*(r02))));
00378 rxp2_2=((((-1.00000000000000)*(px)*(r12)))+(((py)*(r02))));
00379 {
00380 IKReal j3array[2], cj3array[2], sj3array[2];
00381 bool j3valid[2]={false};
00382 cj3array[0]=((-1.00032051282051)+(((3.20512820512821)*(pp))));
00383 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
00384 {
00385     j3valid[0] = j3valid[1] = true;
00386     j3array[0] = IKacos(cj3array[0]);
00387     sj3array[0] = IKsin(j3array[0]);
00388     cj3array[1] = cj3array[0];
00389     j3array[1] = -j3array[0];
00390     sj3array[1] = -sj3array[0];
00391 }
00392 else if( isnan(cj3array[0]) )
00393 {
00394     // probably any value will work
00395     j3valid[0] = true;
00396     cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
00397 }
00398 if( j3valid[0] && j3valid[1] && IKabs(cj3array[0]-cj3array[1]) < 0.0001 && IKabs(sj3array[0]-sj3array[1]) < 0.0001 )
00399 {
00400     j3valid[1]=false;
00401 }
00402 for(int ij3 = 0; ij3 < 2; ++ij3)
00403 {
00404 if( !j3valid[ij3] )
00405 {
00406     continue;
00407 }
00408 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00409 
00410 {
00411 IKReal dummyeval[1];
00412 IKReal gconst0;
00413 IKReal x67=(cj2)*(cj2);
00414 IKReal x68=(py)*(py);
00415 IKReal x69=(sj2)*(sj2);
00416 gconst0=IKsign(((((100.000000000000)*(py)*(((((px)*(py)*(x69)))+(((px)*(py)*(x67)))))))+(((100.000000000000)*(px)*(((((-1.00000000000000)*(x67)*(x68)))+(((-1.00000000000000)*(x68)*(x69)))))))));
00417 dummyeval[0]=0;
00418 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00419 {
00420 {
00421 IKReal dummyeval[1];
00422 dummyeval[0]=(((px)*(px))+((py)*(py)));
00423 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00424 {
00425 {
00426 IKReal dummyeval[1];
00427 dummyeval[0]=((1.05193951347797)+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((2.05128205128205)*(cj3))));
00428 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00429 {
00430 continue;
00431 
00432 } else
00433 {
00434 {
00435 IKReal j1array[2], cj1array[2], sj1array[2];
00436 bool j1valid[2]={false};
00437 IKReal x70=((0.390000000000000)*(cj3));
00438 IKReal x71=((0.400000000000000)+(x70));
00439 IKReal x72=(x71)*(x71);
00440 IKReal x73=(cj2)*(cj2);
00441 IKReal x74=(sj3)*(sj3);
00442 IKReal x75=((0.152100000000000)*(x73)*(x74));
00443 IKReal x76=((x75)+(x72));
00444 if( (x76) < (IKReal)-0.00001 )
00445     continue;
00446 IKReal x77=IKsqrt(x76);
00447 IKReal x78=IKabs(x77);
00448 IKReal x79=((IKabs(x78) != 0)?((IKReal)1/(x78)):(IKReal)1.0e30);
00449 IKReal x80=((pz)*(x79));
00450 if( (x80) < -1-IKFAST_SINCOS_THRESH || (x80) > 1+IKFAST_SINCOS_THRESH )
00451     continue;
00452 IKReal x81=IKasin(x80);
00453 IKReal x82=((-0.400000000000000)+(((-1.00000000000000)*(x70))));
00454 IKReal x83=((-0.390000000000000)*(cj2)*(sj3));
00455 IKReal x84=IKatan2(x82, x83);
00456 j1array[0]=((((-1.00000000000000)*(x84)))+(((-1.00000000000000)*(x81))));
00457 sj1array[0]=IKsin(j1array[0]);
00458 cj1array[0]=IKcos(j1array[0]);
00459 j1array[1]=((3.14159265358979)+(x81)+(((-1.00000000000000)*(x84))));
00460 sj1array[1]=IKsin(j1array[1]);
00461 cj1array[1]=IKcos(j1array[1]);
00462 if( j1array[0] > IKPI )
00463 {
00464     j1array[0]-=IK2PI;
00465 }
00466 else if( j1array[0] < -IKPI )
00467 {    j1array[0]+=IK2PI;
00468 }
00469 j1valid[0] = true;
00470 if( j1array[1] > IKPI )
00471 {
00472     j1array[1]-=IK2PI;
00473 }
00474 else if( j1array[1] < -IKPI )
00475 {    j1array[1]+=IK2PI;
00476 }
00477 j1valid[1] = true;
00478 if( j1valid[0] && j1valid[1] && IKabs(cj1array[0]-cj1array[1]) < 0.0001 && IKabs(sj1array[0]-sj1array[1]) < 0.0001 )
00479 {
00480     j1valid[1]=false;
00481 }
00482 for(int ij1 = 0; ij1 < 2; ++ij1)
00483 {
00484 if( !j1valid[ij1] )
00485 {
00486     continue;
00487 }
00488 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00489 
00490 {
00491 IKReal dummyeval[1];
00492 IKReal gconst1;
00493 gconst1=IKsign(((((-100.000000000000)*(sj1)*((px)*(px))))+(((-100.000000000000)*(sj1)*((py)*(py))))));
00494 dummyeval[0]=((((-1.00000000000000)*(sj1)*((py)*(py))))+(((-1.00000000000000)*(sj1)*((px)*(px)))));
00495 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00496 {
00497 {
00498 IKReal evalcond[5];
00499 IKReal x85=((0.390000000000000)*(cj3));
00500 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j1)), 6.28318530717959)));
00501 evalcond[1]=((0.312100000000000)+(((0.312000000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
00502 evalcond[2]=((-0.400000000000000)+(pz)+(((-1.00000000000000)*(x85))));
00503 evalcond[3]=((-0.00790000000000000)+(((0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
00504 evalcond[4]=((0.400000000000000)+(x85)+(((-1.00000000000000)*(pz))));
00505 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
00506 {
00507 {
00508 IKReal dummyeval[1];
00509 IKReal gconst2;
00510 gconst2=IKsign(((((-100.000000000000)*((py)*(py))))+(((-100.000000000000)*((px)*(px))))));
00511 dummyeval[0]=((((-1.00000000000000)*((px)*(px))))+(((-1.00000000000000)*((py)*(py)))));
00512 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00513 {
00514 continue;
00515 
00516 } else
00517 {
00518 {
00519 IKReal j0array[1], cj0array[1], sj0array[1];
00520 bool j0valid[1]={false};
00521 j0array[0]=IKatan2(((gconst2)*(((((39.0000000000000)*(cj2)*(py)*(sj3)))+(((-39.0000000000000)*(px)*(sj2)*(sj3)))))), ((gconst2)*(((((39.0000000000000)*(py)*(sj2)*(sj3)))+(((39.0000000000000)*(cj2)*(px)*(sj3)))))));
00522 sj0array[0]=IKsin(j0array[0]);
00523 cj0array[0]=IKcos(j0array[0]);
00524 if( j0array[0] > IKPI )
00525 {
00526     j0array[0]-=IK2PI;
00527 }
00528 else if( j0array[0] < -IKPI )
00529 {    j0array[0]+=IK2PI;
00530 }
00531 j0valid[0] = true;
00532 for(int ij0 = 0; ij0 < 1; ++ij0)
00533 {
00534 if( !j0valid[ij0] )
00535 {
00536     continue;
00537 }
00538 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00539 
00540 rotationfunction0(vsolutions);
00541 }
00542 }
00543 
00544 }
00545 
00546 }
00547 
00548 } else
00549 {
00550 IKReal x133=((0.390000000000000)*(cj3));
00551 IKReal x134=((0.400000000000000)+(x133)+(pz));
00552 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j1)), 6.28318530717959)));
00553 evalcond[1]=((0.312100000000000)+(((0.312000000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
00554 evalcond[2]=x134;
00555 evalcond[3]=((-0.00790000000000000)+(((-0.800000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
00556 evalcond[4]=x134;
00557 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  )
00558 {
00559 {
00560 IKReal dummyeval[1];
00561 IKReal gconst3;
00562 gconst3=IKsign(((((100.000000000000)*((py)*(py))))+(((100.000000000000)*((px)*(px))))));
00563 dummyeval[0]=(((px)*(px))+((py)*(py)));
00564 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00565 {
00566 continue;
00567 
00568 } else
00569 {
00570 {
00571 IKReal j0array[1], cj0array[1], sj0array[1];
00572 bool j0valid[1]={false};
00573 j0array[0]=IKatan2(((gconst3)*(((((39.0000000000000)*(px)*(sj2)*(sj3)))+(((39.0000000000000)*(cj2)*(py)*(sj3)))))), ((gconst3)*(((((-39.0000000000000)*(py)*(sj2)*(sj3)))+(((39.0000000000000)*(cj2)*(px)*(sj3)))))));
00574 sj0array[0]=IKsin(j0array[0]);
00575 cj0array[0]=IKcos(j0array[0]);
00576 if( j0array[0] > IKPI )
00577 {
00578     j0array[0]-=IK2PI;
00579 }
00580 else if( j0array[0] < -IKPI )
00581 {    j0array[0]+=IK2PI;
00582 }
00583 j0valid[0] = true;
00584 for(int ij0 = 0; ij0 < 1; ++ij0)
00585 {
00586 if( !j0valid[ij0] )
00587 {
00588     continue;
00589 }
00590 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00591 
00592 rotationfunction0(vsolutions);
00593 }
00594 }
00595 
00596 }
00597 
00598 }
00599 
00600 } else
00601 {
00602 if( 1 )
00603 {
00604 continue;
00605 
00606 } else
00607 {
00608 }
00609 }
00610 }
00611 }
00612 
00613 } else
00614 {
00615 {
00616 IKReal j0array[1], cj0array[1], sj0array[1];
00617 bool j0valid[1]={false};
00618 j0array[0]=IKatan2(((gconst1)*(((((-39.0000000000000)*(px)*(sj1)*(sj2)*(sj3)))+(((100.000000000000)*(cj1)*(py)*(pz)))+(((-125.000000000000)*(pp)*(py)))+(((-0.987500000000000)*(py)))))), ((gconst1)*(((((100.000000000000)*(cj1)*(px)*(pz)))+(((-125.000000000000)*(pp)*(px)))+(((39.0000000000000)*(py)*(sj1)*(sj2)*(sj3)))+(((-0.987500000000000)*(px)))))));
00619 sj0array[0]=IKsin(j0array[0]);
00620 cj0array[0]=IKcos(j0array[0]);
00621 if( j0array[0] > IKPI )
00622 {
00623     j0array[0]-=IK2PI;
00624 }
00625 else if( j0array[0] < -IKPI )
00626 {    j0array[0]+=IK2PI;
00627 }
00628 j0valid[0] = true;
00629 for(int ij0 = 0; ij0 < 1; ++ij0)
00630 {
00631 if( !j0valid[ij0] )
00632 {
00633     continue;
00634 }
00635 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00636 
00637 rotationfunction0(vsolutions);
00638 }
00639 }
00640 
00641 }
00642 
00643 }
00644 }
00645 }
00646 
00647 }
00648 
00649 }
00650 
00651 } else
00652 {
00653 {
00654 IKReal j0array[2], cj0array[2], sj0array[2];
00655 bool j0valid[2]={false};
00656 IKReal x135=((-1.00000000000000)*(py));
00657 IKReal x136=IKatan2(x135, px);
00658 IKReal x137=(px)*(px);
00659 IKReal x138=(py)*(py);
00660 IKReal x139=((x137)+(x138));
00661 if( (x139) < (IKReal)-0.00001 )
00662     continue;
00663 IKReal x140=IKsqrt(x139);
00664 IKReal x141=IKabs(x140);
00665 IKReal x142=((IKabs(x141) != 0)?((IKReal)1/(x141)):(IKReal)1.0e30);
00666 IKReal x143=((0.390000000000000)*(sj2)*(sj3)*(x142));
00667 if( (x143) < -1-IKFAST_SINCOS_THRESH || (x143) > 1+IKFAST_SINCOS_THRESH )
00668     continue;
00669 IKReal x144=IKasin(x143);
00670 j0array[0]=((x144)+(((-1.00000000000000)*(x136))));
00671 sj0array[0]=IKsin(j0array[0]);
00672 cj0array[0]=IKcos(j0array[0]);
00673 j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x144)))+(((-1.00000000000000)*(x136))));
00674 sj0array[1]=IKsin(j0array[1]);
00675 cj0array[1]=IKcos(j0array[1]);
00676 if( j0array[0] > IKPI )
00677 {
00678     j0array[0]-=IK2PI;
00679 }
00680 else if( j0array[0] < -IKPI )
00681 {    j0array[0]+=IK2PI;
00682 }
00683 j0valid[0] = true;
00684 if( j0array[1] > IKPI )
00685 {
00686     j0array[1]-=IK2PI;
00687 }
00688 else if( j0array[1] < -IKPI )
00689 {    j0array[1]+=IK2PI;
00690 }
00691 j0valid[1] = true;
00692 if( j0valid[0] && j0valid[1] && IKabs(cj0array[0]-cj0array[1]) < 0.0001 && IKabs(sj0array[0]-sj0array[1]) < 0.0001 )
00693 {
00694     j0valid[1]=false;
00695 }
00696 for(int ij0 = 0; ij0 < 2; ++ij0)
00697 {
00698 if( !j0valid[ij0] )
00699 {
00700     continue;
00701 }
00702 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00703 
00704 {
00705 IKReal dummyeval[1];
00706 IKReal gconst4;
00707 gconst4=IKsign(((((40.0000000000000)*(py)*(sj0)))+(((40.0000000000000)*(cj0)*(px)))+(((39.0000000000000)*(cj3)*(py)*(sj0)))+(((-39.0000000000000)*(cj2)*(pz)*(sj3)))+(((39.0000000000000)*(cj0)*(cj3)*(px)))));
00708 dummyeval[0]=((((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px)))+(((1.02564102564103)*(py)*(sj0)))+(((1.02564102564103)*(cj0)*(px))));
00709 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00710 {
00711 {
00712 IKReal dummyeval[1];
00713 dummyeval[0]=((((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px)))+(((1.02564102564103)*(py)*(sj0)))+(((1.02564102564103)*(cj0)*(px))));
00714 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00715 {
00716 continue;
00717 
00718 } else
00719 {
00720 {
00721 IKReal j1array[1], cj1array[1], sj1array[1];
00722 bool j1valid[1]={false};
00723 j1array[0]=IKatan2(((((IKabs(((((40.0000000000000)*(py)*(sj0)))+(((40.0000000000000)*(cj0)*(px)))+(((39.0000000000000)*(cj3)*(py)*(sj0)))+(((-39.0000000000000)*(cj2)*(pz)*(sj3)))+(((39.0000000000000)*(cj0)*(cj3)*(px))))) != 0)?((IKReal)1/(((((40.0000000000000)*(py)*(sj0)))+(((40.0000000000000)*(cj0)*(px)))+(((39.0000000000000)*(cj3)*(py)*(sj0)))+(((-39.0000000000000)*(cj2)*(pz)*(sj3)))+(((39.0000000000000)*(cj0)*(cj3)*(px)))))):(IKReal)1.0e30))*(((0.395000000000000)+(((-100.000000000000)*((pz)*(pz))))+(((48.7500000000000)*(cj3)*(pp)))+(((50.0000000000000)*(pp)))+(((0.385125000000000)*(cj3)))))), ((((IKabs(((((156.000000000000)*(cj0)*(cj3)*(px)))+(((-156.000000000000)*(cj2)*(pz)*(sj3)))+(((160.000000000000)*(py)*(sj0)))+(((160.000000000000)*(cj0)*(px)))+(((156.000000000000)*(cj3)*(py)*(sj0))))) != 0)?((IKReal)1/(((((156.000000000000)*(cj0)*(cj3)*(px)))+(((-156.000000000000)*(cj2)*(pz)*(sj3)))+(((160.000000000000)*(py)*(sj0)))+(((160.000000000000)*(cj0)*(px)))+(((156.000000000000)*(cj3)*(py)*(sj0)))))):(IKReal)1.0e30))*(((((-1.54050000000000)*(cj2)*(sj3)))+(((400.000000000000)*(cj0)*(px)*(pz)))+(((-195.000000000000)*(cj2)*(pp)*(sj3)))+(((400.000000000000)*(py)*(pz)*(sj0)))))));
00724 sj1array[0]=IKsin(j1array[0]);
00725 cj1array[0]=IKcos(j1array[0]);
00726 if( j1array[0] > IKPI )
00727 {
00728     j1array[0]-=IK2PI;
00729 }
00730 else if( j1array[0] < -IKPI )
00731 {    j1array[0]+=IK2PI;
00732 }
00733 j1valid[0] = true;
00734 for(int ij1 = 0; ij1 < 1; ++ij1)
00735 {
00736 if( !j1valid[ij1] )
00737 {
00738     continue;
00739 }
00740 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00741 
00742 rotationfunction0(vsolutions);
00743 }
00744 }
00745 
00746 }
00747 
00748 }
00749 
00750 } else
00751 {
00752 {
00753 IKReal j1array[1], cj1array[1], sj1array[1];
00754 bool j1valid[1]={false};
00755 j1array[0]=IKatan2(((gconst4)*(((16.0000000000000)+(((-100.000000000000)*((pz)*(pz))))+(((31.2000000000000)*(cj3)))+(((15.2100000000000)*((cj3)*(cj3))))))), ((gconst4)*(((((100.000000000000)*(cj0)*(px)*(pz)))+(((-15.2100000000000)*(cj2)*(cj3)*(sj3)))+(((-15.6000000000000)*(cj2)*(sj3)))+(((100.000000000000)*(py)*(pz)*(sj0)))))));
00756 sj1array[0]=IKsin(j1array[0]);
00757 cj1array[0]=IKcos(j1array[0]);
00758 if( j1array[0] > IKPI )
00759 {
00760     j1array[0]-=IK2PI;
00761 }
00762 else if( j1array[0] < -IKPI )
00763 {    j1array[0]+=IK2PI;
00764 }
00765 j1valid[0] = true;
00766 for(int ij1 = 0; ij1 < 1; ++ij1)
00767 {
00768 if( !j1valid[ij1] )
00769 {
00770     continue;
00771 }
00772 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00773 
00774 rotationfunction0(vsolutions);
00775 }
00776 }
00777 
00778 }
00779 
00780 }
00781 }
00782 }
00783 
00784 }
00785 
00786 }
00787 
00788 } else
00789 {
00790 {
00791 IKReal j0array[1], cj0array[1], sj0array[1];
00792 bool j0valid[1]={false};
00793 IKReal x145=(py)*(py);
00794 IKReal x146=(sj2)*(sj2);
00795 IKReal x147=(cj2)*(cj2);
00796 j0array[0]=IKatan2(((gconst0)*(((((39.0000000000000)*(sj2)*(sj3)*(x145)))+(((39.0000000000000)*(sj2)*(sj3)*(((((-1.00000000000000)*(x145)*(x146)))+(((-1.00000000000000)*(x145)*(x147)))))))))), ((gconst0)*(((((39.0000000000000)*(px)*(py)*(sj2)*(sj3)))+(((-39.0000000000000)*(sj2)*(sj3)*(((((px)*(py)*(x147)))+(((px)*(py)*(x146)))))))))));
00797 sj0array[0]=IKsin(j0array[0]);
00798 cj0array[0]=IKcos(j0array[0]);
00799 if( j0array[0] > IKPI )
00800 {
00801     j0array[0]-=IK2PI;
00802 }
00803 else if( j0array[0] < -IKPI )
00804 {    j0array[0]+=IK2PI;
00805 }
00806 j0valid[0] = true;
00807 for(int ij0 = 0; ij0 < 1; ++ij0)
00808 {
00809 if( !j0valid[ij0] )
00810 {
00811     continue;
00812 }
00813 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00814 
00815 {
00816 IKReal dummyeval[1];
00817 IKReal gconst4;
00818 gconst4=IKsign(((((40.0000000000000)*(py)*(sj0)))+(((40.0000000000000)*(cj0)*(px)))+(((39.0000000000000)*(cj3)*(py)*(sj0)))+(((-39.0000000000000)*(cj2)*(pz)*(sj3)))+(((39.0000000000000)*(cj0)*(cj3)*(px)))));
00819 dummyeval[0]=((((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px)))+(((1.02564102564103)*(py)*(sj0)))+(((1.02564102564103)*(cj0)*(px))));
00820 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00821 {
00822 {
00823 IKReal dummyeval[1];
00824 dummyeval[0]=((((-1.00000000000000)*(cj2)*(pz)*(sj3)))+(((cj3)*(py)*(sj0)))+(((cj0)*(cj3)*(px)))+(((1.02564102564103)*(py)*(sj0)))+(((1.02564102564103)*(cj0)*(px))));
00825 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00826 {
00827 continue;
00828 
00829 } else
00830 {
00831 {
00832 IKReal j1array[1], cj1array[1], sj1array[1];
00833 bool j1valid[1]={false};
00834 j1array[0]=IKatan2(((((IKabs(((((40.0000000000000)*(py)*(sj0)))+(((40.0000000000000)*(cj0)*(px)))+(((39.0000000000000)*(cj3)*(py)*(sj0)))+(((-39.0000000000000)*(cj2)*(pz)*(sj3)))+(((39.0000000000000)*(cj0)*(cj3)*(px))))) != 0)?((IKReal)1/(((((40.0000000000000)*(py)*(sj0)))+(((40.0000000000000)*(cj0)*(px)))+(((39.0000000000000)*(cj3)*(py)*(sj0)))+(((-39.0000000000000)*(cj2)*(pz)*(sj3)))+(((39.0000000000000)*(cj0)*(cj3)*(px)))))):(IKReal)1.0e30))*(((0.395000000000000)+(((-100.000000000000)*((pz)*(pz))))+(((48.7500000000000)*(cj3)*(pp)))+(((50.0000000000000)*(pp)))+(((0.385125000000000)*(cj3)))))), ((((IKabs(((((156.000000000000)*(cj0)*(cj3)*(px)))+(((-156.000000000000)*(cj2)*(pz)*(sj3)))+(((160.000000000000)*(py)*(sj0)))+(((160.000000000000)*(cj0)*(px)))+(((156.000000000000)*(cj3)*(py)*(sj0))))) != 0)?((IKReal)1/(((((156.000000000000)*(cj0)*(cj3)*(px)))+(((-156.000000000000)*(cj2)*(pz)*(sj3)))+(((160.000000000000)*(py)*(sj0)))+(((160.000000000000)*(cj0)*(px)))+(((156.000000000000)*(cj3)*(py)*(sj0)))))):(IKReal)1.0e30))*(((((-1.54050000000000)*(cj2)*(sj3)))+(((400.000000000000)*(cj0)*(px)*(pz)))+(((-195.000000000000)*(cj2)*(pp)*(sj3)))+(((400.000000000000)*(py)*(pz)*(sj0)))))));
00835 sj1array[0]=IKsin(j1array[0]);
00836 cj1array[0]=IKcos(j1array[0]);
00837 if( j1array[0] > IKPI )
00838 {
00839     j1array[0]-=IK2PI;
00840 }
00841 else if( j1array[0] < -IKPI )
00842 {    j1array[0]+=IK2PI;
00843 }
00844 j1valid[0] = true;
00845 for(int ij1 = 0; ij1 < 1; ++ij1)
00846 {
00847 if( !j1valid[ij1] )
00848 {
00849     continue;
00850 }
00851 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00852 
00853 rotationfunction0(vsolutions);
00854 }
00855 }
00856 
00857 }
00858 
00859 }
00860 
00861 } else
00862 {
00863 {
00864 IKReal j1array[1], cj1array[1], sj1array[1];
00865 bool j1valid[1]={false};
00866 j1array[0]=IKatan2(((gconst4)*(((16.0000000000000)+(((-100.000000000000)*((pz)*(pz))))+(((31.2000000000000)*(cj3)))+(((15.2100000000000)*((cj3)*(cj3))))))), ((gconst4)*(((((100.000000000000)*(cj0)*(px)*(pz)))+(((-15.2100000000000)*(cj2)*(cj3)*(sj3)))+(((-15.6000000000000)*(cj2)*(sj3)))+(((100.000000000000)*(py)*(pz)*(sj0)))))));
00867 sj1array[0]=IKsin(j1array[0]);
00868 cj1array[0]=IKcos(j1array[0]);
00869 if( j1array[0] > IKPI )
00870 {
00871     j1array[0]-=IK2PI;
00872 }
00873 else if( j1array[0] < -IKPI )
00874 {    j1array[0]+=IK2PI;
00875 }
00876 j1valid[0] = true;
00877 for(int ij1 = 0; ij1 < 1; ++ij1)
00878 {
00879 if( !j1valid[ij1] )
00880 {
00881     continue;
00882 }
00883 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00884 
00885 rotationfunction0(vsolutions);
00886 }
00887 }
00888 
00889 }
00890 
00891 }
00892 }
00893 }
00894 
00895 }
00896 
00897 }
00898 }
00899 }
00900 }
00901 return vsolutions.size()>0;
00902 }
00903 inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
00904 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
00905 IKReal x86=((sj1)*(sj3));
00906 IKReal x87=((cj1)*(cj2)*(cj3));
00907 IKReal x88=((x86)+(x87));
00908 IKReal x89=((cj0)*(cj3)*(sj2));
00909 IKReal x90=((sj0)*(x88));
00910 IKReal x91=((x89)+(x90));
00911 IKReal x92=((cj0)*(x88));
00912 IKReal x93=((cj3)*(sj0)*(sj2));
00913 IKReal x94=((x92)+(((-1.00000000000000)*(x93))));
00914 IKReal x95=((cj1)*(sj3));
00915 IKReal x96=((cj2)*(cj3)*(sj1));
00916 IKReal x97=((x95)+(((-1.00000000000000)*(x96))));
00917 IKReal x98=((cj0)*(cj2));
00918 IKReal x99=((cj1)*(sj0)*(sj2));
00919 IKReal x100=((x98)+(((-1.00000000000000)*(x99))));
00920 IKReal x101=((cj2)*(sj0));
00921 IKReal x102=((cj0)*(cj1)*(sj2));
00922 IKReal x103=((x101)+(x102));
00923 IKReal x104=((-1.00000000000000)*(x103));
00924 IKReal x105=((cj3)*(sj1));
00925 IKReal x106=((cj2)*(x95));
00926 IKReal x107=((((-1.00000000000000)*(x106)))+(x105));
00927 IKReal x108=((cj1)*(cj3));
00928 IKReal x109=((cj2)*(x86));
00929 IKReal x110=((x108)+(x109));
00930 IKReal x111=((cj0)*(x107));
00931 IKReal x112=((sj0)*(sj2)*(sj3));
00932 IKReal x113=((x112)+(x111));
00933 IKReal x114=((sj0)*(x107));
00934 IKReal x115=((-1.00000000000000)*(cj0)*(sj2)*(sj3));
00935 IKReal x116=((x115)+(x114));
00936 new_r00=((((r10)*(x91)))+(((r00)*(x94)))+(((r20)*(x97))));
00937 new_r01=((((r21)*(x97)))+(((r11)*(x91)))+(((r01)*(x94))));
00938 new_r02=((((r12)*(x91)))+(((r02)*(x94)))+(((r22)*(x97))));
00939 new_r10=((((r10)*(x100)))+(((r00)*(x104)))+(((r20)*(sj1)*(sj2))));
00940 new_r11=((((r01)*(x104)))+(((r11)*(x100)))+(((r21)*(sj1)*(sj2))));
00941 new_r12=((((r02)*(x104)))+(((r12)*(x100)))+(((r22)*(sj1)*(sj2))));
00942 new_r20=((((r10)*(x116)))+(((r20)*(x110)))+(((r00)*(x113))));
00943 new_r21=((((r11)*(((x114)+(((-1.00000000000000)*(cj0)*(sj2)*(sj3)))))))+(((r21)*(x110)))+(((r01)*(x113))));
00944 new_r22=((((r22)*(x110)))+(((r02)*(x113)))+(((r12)*(x116))));
00945 {
00946 IKReal j5array[2], cj5array[2], sj5array[2];
00947 bool j5valid[2]={false};
00948 cj5array[0]=new_r22;
00949 if( cj5array[0] >= -1-IKFAST_SINCOS_THRESH && cj5array[0] <= 1+IKFAST_SINCOS_THRESH )
00950 {
00951     j5valid[0] = j5valid[1] = true;
00952     j5array[0] = IKacos(cj5array[0]);
00953     sj5array[0] = IKsin(j5array[0]);
00954     cj5array[1] = cj5array[0];
00955     j5array[1] = -j5array[0];
00956     sj5array[1] = -sj5array[0];
00957 }
00958 else if( isnan(cj5array[0]) )
00959 {
00960     // probably any value will work
00961     j5valid[0] = true;
00962     cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
00963 }
00964 if( j5valid[0] && j5valid[1] && IKabs(cj5array[0]-cj5array[1]) < 0.0001 && IKabs(sj5array[0]-sj5array[1]) < 0.0001 )
00965 {
00966     j5valid[1]=false;
00967 }
00968 for(int ij5 = 0; ij5 < 2; ++ij5)
00969 {
00970 if( !j5valid[ij5] )
00971 {
00972     continue;
00973 }
00974 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00975 
00976 {
00977 IKReal dummyeval[1];
00978 IKReal gconst6;
00979 gconst6=IKsign(sj5);
00980 dummyeval[0]=sj5;
00981 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00982 {
00983 {
00984 IKReal dummyeval[1];
00985 IKReal gconst5;
00986 gconst5=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
00987 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
00988 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
00989 {
00990 {
00991 IKReal evalcond[7];
00992 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j5)), 6.28318530717959)));
00993 evalcond[1]=((-1.00000000000000)+(new_r22));
00994 evalcond[2]=new_r20;
00995 evalcond[3]=new_r21;
00996 evalcond[4]=((-1.00000000000000)*(new_r20));
00997 evalcond[5]=((-1.00000000000000)*(new_r21));
00998 evalcond[6]=((1.00000000000000)+(((-1.00000000000000)*(new_r22))));
00999 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
01000 {
01001 {
01002 IKReal j4array[2], cj4array[2], sj4array[2];
01003 bool j4valid[2]={false};
01004 IKReal x117=IKatan2(new_r02, new_r12);
01005 j4array[0]=((-1.00000000000000)*(x117));
01006 sj4array[0]=IKsin(j4array[0]);
01007 cj4array[0]=IKcos(j4array[0]);
01008 j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x117))));
01009 sj4array[1]=IKsin(j4array[1]);
01010 cj4array[1]=IKcos(j4array[1]);
01011 if( j4array[0] > IKPI )
01012 {
01013     j4array[0]-=IK2PI;
01014 }
01015 else if( j4array[0] < -IKPI )
01016 {    j4array[0]+=IK2PI;
01017 }
01018 j4valid[0] = true;
01019 if( j4array[1] > IKPI )
01020 {
01021     j4array[1]-=IK2PI;
01022 }
01023 else if( j4array[1] < -IKPI )
01024 {    j4array[1]+=IK2PI;
01025 }
01026 j4valid[1] = true;
01027 if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
01028 {
01029     j4valid[1]=false;
01030 }
01031 for(int ij4 = 0; ij4 < 2; ++ij4)
01032 {
01033 if( !j4valid[ij4] )
01034 {
01035     continue;
01036 }
01037 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01038 
01039 {
01040 IKReal j6array[1], cj6array[1], sj6array[1];
01041 bool j6valid[1]={false};
01042 j6array[0]=IKatan2(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))), ((((cj4)*(new_r00)))+(((new_r10)*(sj4)))));
01043 sj6array[0]=IKsin(j6array[0]);
01044 cj6array[0]=IKcos(j6array[0]);
01045 if( j6array[0] > IKPI )
01046 {
01047     j6array[0]-=IK2PI;
01048 }
01049 else if( j6array[0] < -IKPI )
01050 {    j6array[0]+=IK2PI;
01051 }
01052 j6valid[0] = true;
01053 for(int ij6 = 0; ij6 < 1; ++ij6)
01054 {
01055 if( !j6valid[ij6] )
01056 {
01057     continue;
01058 }
01059 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01060 
01061 {
01062 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01063 solution.basesol.resize(7);
01064 solution.basesol[0].foffset = j0;
01065 solution.basesol[1].foffset = j1;
01066 solution.basesol[2].foffset = j2;
01067 solution.basesol[3].foffset = j3;
01068 solution.basesol[4].foffset = j4;
01069 solution.basesol[5].foffset = j5;
01070 solution.basesol[6].foffset = j6;
01071 solution.vfree.resize(0);
01072 }
01073 }
01074 }
01075 }
01076 }
01077 
01078 } else
01079 {
01080 IKReal x118=((1.00000000000000)+(new_r22));
01081 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
01082 evalcond[1]=x118;
01083 evalcond[2]=new_r20;
01084 evalcond[3]=new_r21;
01085 evalcond[4]=new_r20;
01086 evalcond[5]=new_r21;
01087 evalcond[6]=x118;
01088 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  )
01089 {
01090 {
01091 IKReal j4array[2], cj4array[2], sj4array[2];
01092 bool j4valid[2]={false};
01093 IKReal x119=IKatan2(new_r02, new_r12);
01094 j4array[0]=((-1.00000000000000)*(x119));
01095 sj4array[0]=IKsin(j4array[0]);
01096 cj4array[0]=IKcos(j4array[0]);
01097 j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x119))));
01098 sj4array[1]=IKsin(j4array[1]);
01099 cj4array[1]=IKcos(j4array[1]);
01100 if( j4array[0] > IKPI )
01101 {
01102     j4array[0]-=IK2PI;
01103 }
01104 else if( j4array[0] < -IKPI )
01105 {    j4array[0]+=IK2PI;
01106 }
01107 j4valid[0] = true;
01108 if( j4array[1] > IKPI )
01109 {
01110     j4array[1]-=IK2PI;
01111 }
01112 else if( j4array[1] < -IKPI )
01113 {    j4array[1]+=IK2PI;
01114 }
01115 j4valid[1] = true;
01116 if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
01117 {
01118     j4valid[1]=false;
01119 }
01120 for(int ij4 = 0; ij4 < 2; ++ij4)
01121 {
01122 if( !j4valid[ij4] )
01123 {
01124     continue;
01125 }
01126 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01127 
01128 {
01129 IKReal j6array[1], cj6array[1], sj6array[1];
01130 bool j6valid[1]={false};
01131 j6array[0]=IKatan2(((((cj4)*(new_r01)))+(((new_r11)*(sj4)))), ((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))));
01132 sj6array[0]=IKsin(j6array[0]);
01133 cj6array[0]=IKcos(j6array[0]);
01134 if( j6array[0] > IKPI )
01135 {
01136     j6array[0]-=IK2PI;
01137 }
01138 else if( j6array[0] < -IKPI )
01139 {    j6array[0]+=IK2PI;
01140 }
01141 j6valid[0] = true;
01142 for(int ij6 = 0; ij6 < 1; ++ij6)
01143 {
01144 if( !j6valid[ij6] )
01145 {
01146     continue;
01147 }
01148 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01149 
01150 {
01151 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01152 solution.basesol.resize(7);
01153 solution.basesol[0].foffset = j0;
01154 solution.basesol[1].foffset = j1;
01155 solution.basesol[2].foffset = j2;
01156 solution.basesol[3].foffset = j3;
01157 solution.basesol[4].foffset = j4;
01158 solution.basesol[5].foffset = j5;
01159 solution.basesol[6].foffset = j6;
01160 solution.vfree.resize(0);
01161 }
01162 }
01163 }
01164 }
01165 }
01166 
01167 } else
01168 {
01169 if( 1 )
01170 {
01171 continue;
01172 
01173 } else
01174 {
01175 }
01176 }
01177 }
01178 }
01179 
01180 } else
01181 {
01182 {
01183 IKReal j4array[1], cj4array[1], sj4array[1];
01184 bool j4valid[1]={false};
01185 j4array[0]=IKatan2(((gconst5)*(new_r12)*(sj5)), ((gconst5)*(new_r02)*(sj5)));
01186 sj4array[0]=IKsin(j4array[0]);
01187 cj4array[0]=IKcos(j4array[0]);
01188 if( j4array[0] > IKPI )
01189 {
01190     j4array[0]-=IK2PI;
01191 }
01192 else if( j4array[0] < -IKPI )
01193 {    j4array[0]+=IK2PI;
01194 }
01195 j4valid[0] = true;
01196 for(int ij4 = 0; ij4 < 1; ++ij4)
01197 {
01198 if( !j4valid[ij4] )
01199 {
01200     continue;
01201 }
01202 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01203 
01204 {
01205 IKReal dummyeval[1];
01206 IKReal gconst7;
01207 gconst7=IKsign(sj5);
01208 dummyeval[0]=sj5;
01209 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01210 {
01211 {
01212 IKReal evalcond[11];
01213 IKReal x120=((cj4)*(new_r12));
01214 IKReal x121=((new_r02)*(sj4));
01215 IKReal x122=((((-1.00000000000000)*(x121)))+(x120));
01216 IKReal x123=((new_r12)*(sj4));
01217 IKReal x124=((cj4)*(new_r02));
01218 IKReal x125=((x124)+(x123));
01219 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j5)), 6.28318530717959)));
01220 evalcond[1]=((-1.00000000000000)+(new_r22));
01221 evalcond[2]=new_r20;
01222 evalcond[3]=new_r21;
01223 evalcond[4]=x122;
01224 evalcond[5]=x122;
01225 evalcond[6]=x125;
01226 evalcond[7]=x125;
01227 evalcond[8]=((-1.00000000000000)*(new_r20));
01228 evalcond[9]=((-1.00000000000000)*(new_r21));
01229 evalcond[10]=((1.00000000000000)+(((-1.00000000000000)*(new_r22))));
01230 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
01231 {
01232 {
01233 IKReal j6array[1], cj6array[1], sj6array[1];
01234 bool j6valid[1]={false};
01235 j6array[0]=IKatan2(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))), ((((cj4)*(new_r00)))+(((new_r10)*(sj4)))));
01236 sj6array[0]=IKsin(j6array[0]);
01237 cj6array[0]=IKcos(j6array[0]);
01238 if( j6array[0] > IKPI )
01239 {
01240     j6array[0]-=IK2PI;
01241 }
01242 else if( j6array[0] < -IKPI )
01243 {    j6array[0]+=IK2PI;
01244 }
01245 j6valid[0] = true;
01246 for(int ij6 = 0; ij6 < 1; ++ij6)
01247 {
01248 if( !j6valid[ij6] )
01249 {
01250     continue;
01251 }
01252 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01253 
01254 {
01255 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01256 solution.basesol.resize(7);
01257 solution.basesol[0].foffset = j0;
01258 solution.basesol[1].foffset = j1;
01259 solution.basesol[2].foffset = j2;
01260 solution.basesol[3].foffset = j3;
01261 solution.basesol[4].foffset = j4;
01262 solution.basesol[5].foffset = j5;
01263 solution.basesol[6].foffset = j6;
01264 solution.vfree.resize(0);
01265 }
01266 }
01267 }
01268 
01269 } else
01270 {
01271 IKReal x126=((cj4)*(new_r12));
01272 IKReal x127=((new_r02)*(sj4));
01273 IKReal x128=((((-1.00000000000000)*(x127)))+(x126));
01274 IKReal x129=((new_r12)*(sj4));
01275 IKReal x130=((cj4)*(new_r02));
01276 IKReal x131=((x130)+(x129));
01277 IKReal x132=((1.00000000000000)+(new_r22));
01278 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
01279 evalcond[1]=x132;
01280 evalcond[2]=new_r20;
01281 evalcond[3]=new_r21;
01282 evalcond[4]=x128;
01283 evalcond[5]=x128;
01284 evalcond[6]=x131;
01285 evalcond[7]=((-1.00000000000000)*(x131));
01286 evalcond[8]=new_r20;
01287 evalcond[9]=new_r21;
01288 evalcond[10]=x132;
01289 if( IKabs(evalcond[0]) < 0.0000010000000000  && IKabs(evalcond[1]) < 0.0000010000000000  && IKabs(evalcond[2]) < 0.0000010000000000  && IKabs(evalcond[3]) < 0.0000010000000000  && IKabs(evalcond[4]) < 0.0000010000000000  && IKabs(evalcond[5]) < 0.0000010000000000  && IKabs(evalcond[6]) < 0.0000010000000000  && IKabs(evalcond[7]) < 0.0000010000000000  && IKabs(evalcond[8]) < 0.0000010000000000  && IKabs(evalcond[9]) < 0.0000010000000000  && IKabs(evalcond[10]) < 0.0000010000000000  )
01290 {
01291 {
01292 IKReal j6array[1], cj6array[1], sj6array[1];
01293 bool j6valid[1]={false};
01294 j6array[0]=IKatan2(((((cj4)*(new_r01)))+(((new_r11)*(sj4)))), ((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))));
01295 sj6array[0]=IKsin(j6array[0]);
01296 cj6array[0]=IKcos(j6array[0]);
01297 if( j6array[0] > IKPI )
01298 {
01299     j6array[0]-=IK2PI;
01300 }
01301 else if( j6array[0] < -IKPI )
01302 {    j6array[0]+=IK2PI;
01303 }
01304 j6valid[0] = true;
01305 for(int ij6 = 0; ij6 < 1; ++ij6)
01306 {
01307 if( !j6valid[ij6] )
01308 {
01309     continue;
01310 }
01311 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01312 
01313 {
01314 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01315 solution.basesol.resize(7);
01316 solution.basesol[0].foffset = j0;
01317 solution.basesol[1].foffset = j1;
01318 solution.basesol[2].foffset = j2;
01319 solution.basesol[3].foffset = j3;
01320 solution.basesol[4].foffset = j4;
01321 solution.basesol[5].foffset = j5;
01322 solution.basesol[6].foffset = j6;
01323 solution.vfree.resize(0);
01324 }
01325 }
01326 }
01327 
01328 } else
01329 {
01330 if( 1 )
01331 {
01332 continue;
01333 
01334 } else
01335 {
01336 }
01337 }
01338 }
01339 }
01340 
01341 } else
01342 {
01343 {
01344 IKReal j6array[1], cj6array[1], sj6array[1];
01345 bool j6valid[1]={false};
01346 j6array[0]=IKatan2(((gconst7)*(new_r21)), ((-1.00000000000000)*(gconst7)*(new_r20)));
01347 sj6array[0]=IKsin(j6array[0]);
01348 cj6array[0]=IKcos(j6array[0]);
01349 if( j6array[0] > IKPI )
01350 {
01351     j6array[0]-=IK2PI;
01352 }
01353 else if( j6array[0] < -IKPI )
01354 {    j6array[0]+=IK2PI;
01355 }
01356 j6valid[0] = true;
01357 for(int ij6 = 0; ij6 < 1; ++ij6)
01358 {
01359 if( !j6valid[ij6] )
01360 {
01361     continue;
01362 }
01363 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01364 
01365 {
01366 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01367 solution.basesol.resize(7);
01368 solution.basesol[0].foffset = j0;
01369 solution.basesol[1].foffset = j1;
01370 solution.basesol[2].foffset = j2;
01371 solution.basesol[3].foffset = j3;
01372 solution.basesol[4].foffset = j4;
01373 solution.basesol[5].foffset = j5;
01374 solution.basesol[6].foffset = j6;
01375 solution.vfree.resize(0);
01376 }
01377 }
01378 }
01379 
01380 }
01381 
01382 }
01383 }
01384 }
01385 
01386 }
01387 
01388 }
01389 
01390 } else
01391 {
01392 {
01393 IKReal j6array[1], cj6array[1], sj6array[1];
01394 bool j6valid[1]={false};
01395 j6array[0]=IKatan2(((gconst6)*(new_r21)), ((-1.00000000000000)*(gconst6)*(new_r20)));
01396 sj6array[0]=IKsin(j6array[0]);
01397 cj6array[0]=IKcos(j6array[0]);
01398 if( j6array[0] > IKPI )
01399 {
01400     j6array[0]-=IK2PI;
01401 }
01402 else if( j6array[0] < -IKPI )
01403 {    j6array[0]+=IK2PI;
01404 }
01405 j6valid[0] = true;
01406 for(int ij6 = 0; ij6 < 1; ++ij6)
01407 {
01408 if( !j6valid[ij6] )
01409 {
01410     continue;
01411 }
01412 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01413 
01414 {
01415 IKReal dummyeval[1];
01416 IKReal gconst8;
01417 gconst8=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
01418 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
01419 if( IKabs(dummyeval[0]) < 0.0000010000000000  )
01420 {
01421 continue;
01422 
01423 } else
01424 {
01425 {
01426 IKReal j4array[1], cj4array[1], sj4array[1];
01427 bool j4valid[1]={false};
01428 j4array[0]=IKatan2(((gconst8)*(new_r12)*(sj5)), ((gconst8)*(new_r02)*(sj5)));
01429 sj4array[0]=IKsin(j4array[0]);
01430 cj4array[0]=IKcos(j4array[0]);
01431 if( j4array[0] > IKPI )
01432 {
01433     j4array[0]-=IK2PI;
01434 }
01435 else if( j4array[0] < -IKPI )
01436 {    j4array[0]+=IK2PI;
01437 }
01438 j4valid[0] = true;
01439 for(int ij4 = 0; ij4 < 1; ++ij4)
01440 {
01441 if( !j4valid[ij4] )
01442 {
01443     continue;
01444 }
01445 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01446 
01447 {
01448 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01449 solution.basesol.resize(7);
01450 solution.basesol[0].foffset = j0;
01451 solution.basesol[1].foffset = j1;
01452 solution.basesol[2].foffset = j2;
01453 solution.basesol[3].foffset = j3;
01454 solution.basesol[4].foffset = j4;
01455 solution.basesol[5].foffset = j5;
01456 solution.basesol[6].foffset = j6;
01457 solution.vfree.resize(0);
01458 }
01459 }
01460 }
01461 
01462 }
01463 
01464 }
01465 }
01466 }
01467 
01468 }
01469 
01470 }
01471 }
01472 }
01473 }
01474 }};
01475 
01476 
01479 IKFAST_API bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
01480 IKSolver solver;
01481 return solver.ik(eetrans,eerot,pfree,vsolutions);
01482 }
01483 
01484 IKFAST_API const char* getKinematicsHash() { return "<robot:GenericRobot - WAM7 (4b156a21dd193e47475ebd0a901673bc)>"; }
01485 
01486 #ifdef IKFAST_NAMESPACE
01487 } // end namespace
01488 #endif
01489 
01490 #ifndef IKFAST_NO_MAIN
01491 #include <stdio.h>
01492 #include <stdlib.h>
01493 #ifdef IKFAST_NAMESPACE
01494 using namespace IKFAST_NAMESPACE;
01495 #endif
01496 int main(int argc, char** argv)
01497 {
01498     if( argc != 12+getNumFreeParameters()+1 ) {
01499         printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
01500                "Returns the ik solutions given the transformation of the end effector specified by\n"
01501                "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
01502                "There are %d free parameters that have to be specified.\n\n",getNumFreeParameters());
01503         return 1;
01504     }
01505 
01506     std::vector<IKSolution> vsolutions;
01507     std::vector<IKReal> vfree(getNumFreeParameters());
01508     IKReal eerot[9],eetrans[3];
01509     eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
01510     eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
01511     eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
01512     for(std::size_t i = 0; i < vfree.size(); ++i)
01513         vfree[i] = atof(argv[13+i]);
01514     bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
01515 
01516     if( !bSuccess ) {
01517         fprintf(stderr,"Failed to get ik solution\n");
01518         return -1;
01519     }
01520 
01521     printf("Found %d ik solutions:\n", (int)vsolutions.size());
01522     std::vector<IKReal> sol(getNumJoints());
01523     for(std::size_t i = 0; i < vsolutions.size(); ++i) {
01524         printf("sol%d (free=%d): ", (int)i, (int)vsolutions[i].GetFree().size());
01525         std::vector<IKReal> vsolfree(vsolutions[i].GetFree().size());
01526         vsolutions[i].GetSolution(&sol[0],vsolfree.size()>0?&vsolfree[0]:NULL);
01527         for( std::size_t j = 0; j < sol.size(); ++j)
01528             printf("%.15f, ", sol[j]);
01529         printf("\n");
01530     }
01531     return 0;
01532 }
01533 
01534 #endif

lwr_kinematics
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 02:00:38