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
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
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;
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 );
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 );
00155 if( f <= -1 ) return -IKPI_2;
00156 else if( f >= 1 ) return IKPI_2;
00157 return asin(f);
00158 }
00159
00160
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
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 );
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 );
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));
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));
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,x67,x68,x69,x70,x71,x72,x73,x74,x75,x76,x77,x78,x79,x80,x81,x82,x83,x84,x85,x86,x87,x88,x89,x90,x91,x92,x93,x94,x95,x96,x97,x98,x99,x100,x101,x102,x103,x104,x105,x106,x107,x108,x109,x110,x111,x112,x113,x114,x115,x116,x117;
00245 x0=IKcos(j[1]);
00246 x1=IKsin(j[3]);
00247 x2=IKcos(j[2]);
00248 x3=IKcos(j[3]);
00249 x4=IKsin(j[1]);
00250 x5=IKsin(j[4]);
00251 x6=((x0)*(x1));
00252 x7=((x2)*(x3)*(x4));
00253 x8=((x7)+(x6));
00254 x9=IKcos(j[4]);
00255 x10=IKsin(j[2]);
00256 x11=IKsin(j[6]);
00257 x12=IKsin(j[0]);
00258 x13=IKcos(j[0]);
00259 x14=IKcos(j[6]);
00260 x15=IKcos(j[5]);
00261 x16=((x10)*(x13));
00262 x17=((x0)*(x12)*(x2));
00263 x18=((x17)+(x16));
00264 x19=((-1.00000000000000)*(x18));
00265 x20=((x19)*(x3));
00266 x21=((x1)*(x12)*(x4));
00267 x22=((x20)+(x21));
00268 x23=((x0)*(x10)*(x12));
00269 x24=((x13)*(x2));
00270 x25=((((-1.00000000000000)*(x24)))+(x23));
00271 x26=IKsin(j[5]);
00272 x27=((x0)*(x24));
00273 x28=((x10)*(x12));
00274 x29=((x12)*(x2));
00275 x30=((x0)*(x16));
00276 x31=((x30)+(x29));
00277 x32=((((-1.00000000000000)*(x27)))+(x28));
00278 x33=((-1.00000000000000)*(x8));
00279 x34=((x33)*(x5));
00280 x35=((x10)*(x4)*(x9));
00281 x36=((((-1.00000000000000)*(x35)))+(x34));
00282 x37=((x0)*(x3));
00283 x38=((x1)*(x2)*(x4));
00284 x39=((((-1.00000000000000)*(x38)))+(x37));
00285 x40=((x26)*(x39));
00286 x41=((x8)*(x9));
00287 x42=((x10)*(x4)*(x5));
00288 x43=((((-1.00000000000000)*(x42)))+(x41));
00289 x44=((x15)*(x43));
00290 x45=((x44)+(x40));
00291 x46=((x31)*(x9));
00292 x47=((-1.00000000000000)*(x3)*(x32));
00293 x48=((-1.00000000000000)*(x1)*(x13)*(x4));
00294 x49=((x48)+(x47));
00295 x50=((x49)*(x5));
00296 x51=((x46)+(x50));
00297 x52=((x22)*(x9));
00298 x53=((x25)*(x5));
00299 x54=((x53)+(x52));
00300 x55=((x15)*(x54));
00301 x56=((x1)*(x18));
00302 x57=((x12)*(x3)*(x4));
00303 x58=((x57)+(x56));
00304 x59=((x26)*(x58));
00305 x60=((x59)+(x55));
00306 x61=((x25)*(x9));
00307 x62=((x1)*(x13)*(x4));
00308 x63=((x3)*(x32));
00309 x64=((x62)+(x63));
00310 x65=((x64)*(x9));
00311 x66=((x31)*(x5));
00312 x67=((x65)+(x66));
00313 x68=((x15)*(x67));
00314 x69=((x13)*(x3)*(x4));
00315 x70=((((-1.00000000000000)*(x28)))+(x27));
00316 x71=((x1)*(x70));
00317 x72=((x71)+(x69));
00318 x73=((x26)*(x72));
00319 x74=((x73)+(x68));
00320 x75=((-1.00000000000000)*(x20));
00321 x76=((-1.00000000000000)*(x21));
00322 x77=((x76)+(x75));
00323 x78=((x5)*(x77));
00324 x79=((x78)+(x61));
00325 x80=((-1.00000000000000)*(x60));
00326 x81=((-1.00000000000000)*(x45));
00327 x82=((-1.00000000000000)*(x64));
00328 x83=((x5)*(x82));
00329 x84=((x46)+(x83));
00330 x85=((-1.00000000000000)*(x54));
00331 x86=((((-1.00000000000000)*(x41)))+(x42));
00332 x87=((-1.00000000000000)*(x67));
00333 x88=((0.316500000000000)*(x16));
00334 x89=((0.316500000000000)*(x17));
00335 x90=((x88)+(x89));
00336 x91=((-1.00000000000000)*(x90));
00337 x92=((0.192700000000000)*(x41));
00338 x93=((0.192700000000000)*(x42));
00339 x94=((x92)+(((-1.00000000000000)*(x93))));
00340 x95=((0.192700000000000)*(x38));
00341 x96=((0.192700000000000)*(x37));
00342 x97=((x95)+(((-1.00000000000000)*(x96))));
00343 x98=((0.192700000000000)*(x57));
00344 x99=((0.192700000000000)*(x56));
00345 x100=((x99)+(x98));
00346 x101=((-1.00000000000000)*(x100));
00347 x102=((0.192700000000000)*(x52));
00348 x103=((0.192700000000000)*(x53));
00349 x104=((x102)+(x103));
00350 x105=((0.192700000000000)*(x65));
00351 x106=((0.192700000000000)*(x66));
00352 x107=((x105)+(x106));
00353 x108=((0.316500000000000)*(x28));
00354 x109=((0.316500000000000)*(x27));
00355 x110=((((-1.00000000000000)*(x109)))+(x108));
00356 x111=((0.192700000000000)*(x69));
00357 x112=((0.192700000000000)*(x71));
00358 x113=((x112)+(x111));
00359 x114=((-1.00000000000000)*(x113));
00360 x115=((-1.00000000000000)*(x68));
00361 x116=((-1.00000000000000)*(x73));
00362 x117=((x116)+(x115));
00363 eerot[0]=((((0.183010304668818)*(x14)*(x45)))+(((-0.183020000548001)*(x11)*(x51)))+(((-0.965924897590095)*(x14)*(x60)))+(((-0.183020000548001)*(x14)*(x74)))+(((0.183010304668818)*(x11)*(x36)))+(((-0.965924897590095)*(x11)*(((((-1.00000000000000)*(x22)*(x5)))+(x61))))));
00364 eerot[1]=((((-0.965924897590095)*(x14)*(x79)))+(((0.183010304668818)*(x11)*(x81)))+(((0.183020000548001)*(x11)*(x74)))+(((-0.183020000548001)*(x14)*(x51)))+(((-0.965924897590095)*(x11)*(x80)))+(((0.183010304668818)*(x14)*(x36))));
00365 eerot[2]=((((-0.965924897590095)*(x15)*(x58)))+(((0.183010304668818)*(x15)*(x39)))+(((-0.183020000548001)*(x15)*(x72)))+(((-0.183020000548001)*(x26)*(x87)))+(((0.183010304668818)*(x26)*(x86)))+(((-0.965924897590095)*(x26)*(x85))));
00366 eetrans[0]=((-0.00575257616720444)+(((-0.965924897590095)*(x1)*(x91)))+(((0.0746721602235844)*(x13)*(x4)))+(((-0.183020000548001)*(x1)*(x110)))+(((0.183010304668818)*(x15)*(x97)))+(((-0.183020000548001)*(x114)*(x15)))+(((-0.0579227614276808)*(x37)))+(((-0.965924897590095)*(x101)*(x15)))+(((0.305715230087265)*(x57)))+(((0.183010304668818)*(x26)*(x94)))+(((-0.0746682043048776)*(x0)))+(((0.0579227614276808)*(x38)))+(((-0.183020000548001)*(x107)*(x26)))+(((0.394097358216759)*(x12)*(x4)))+(((0.0579258301734423)*(x69)))+(((-0.965924897590095)*(x104)*(x26))));
00367 eerot[3]=((((-0.683015503672999)*(x11)*(x84)))+(((-0.683015503672999)*(x14)*(x74)))+(((0.258822510973972)*(x14)*(x60)))+(((0.683008586736249)*(x14)*(x45)))+(((0.683008586736249)*(x11)*(x36)))+(((0.258822510973972)*(x11)*(x79))));
00368 eerot[4]=((((-0.683015503672999)*(x11)*(x117)))+(((0.258822510973972)*(x14)*(x79)))+(((0.683008586736249)*(x14)*(x36)))+(((-0.683015503672999)*(x14)*(x84)))+(((0.258822510973972)*(x11)*(x80)))+(((0.683008586736249)*(x11)*(x81))));
00369 eerot[5]=((((0.683008586736249)*(x26)*(x86)))+(((0.683008586736249)*(x15)*(x39)))+(((-0.683015503672999)*(x15)*(x72)))+(((0.258822510973972)*(x26)*(x85)))+(((-0.683015503672999)*(x26)*(x87)))+(((0.258822510973972)*(x15)*(x58))));
00370 eetrans[1]=((-0.175752146684062)+(((0.278670325498584)*(x13)*(x4)))+(((0.216174406912504)*(x69)))+(((-0.216172217702023)*(x37)))+(((0.258822510973972)*(x101)*(x15)))+(((-0.683015503672999)*(x114)*(x15)))+(((0.683008586736249)*(x15)*(x97)))+(((-0.683015503672999)*(x1)*(x110)))+(((0.258822510973972)*(x1)*(x91)))+(((-0.105599584477380)*(x12)*(x4)))+(((0.216172217702023)*(x38)))+(((0.258822510973972)*(x104)*(x26)))+(((-0.0819173247232621)*(x57)))+(((-0.683015503672999)*(x107)*(x26)))+(((-0.278667503388390)*(x0)))+(((0.683008586736249)*(x26)*(x94))));
00371 eerot[6]=((((-0.707111376538080)*(x14)*(x45)))+(((-0.707111376538080)*(x11)*(x36)))+(((-0.707102185784862)*(x14)*(x74)))+(((5.35649803617952e-6)*(x11)*(x79)))+(((-0.707102185784862)*(x11)*(x51)))+(((5.35649803617952e-6)*(x14)*(x60))));
00372 eerot[7]=((((5.35649803617952e-6)*(x14)*(x79)))+(((-0.707102185784862)*(x11)*(x117)))+(((-0.707102185784862)*(x14)*(x84)))+(((-0.707111376538080)*(x14)*(x36)))+(((-0.707111376538080)*(x11)*(x81)))+(((5.35649803617952e-6)*(x11)*(x80))));
00373 eerot[8]=((((-0.707102185784862)*(x15)*(x72)))+(((5.35649803617952e-6)*(x26)*(x85)))+(((-0.707111376538080)*(x26)*(x86)))+(((-0.707102185784862)*(x26)*(x87)))+(((-0.707111376538080)*(x15)*(x39)))+(((5.35649803617952e-6)*(x15)*(x58))));
00374 eetrans[2]=((1.01314784413452)+(((-2.18545119876124e-6)*(x12)*(x4)))+(((5.35649803617952e-6)*(x104)*(x26)))+(((-0.707102185784862)*(x1)*(x110)))+(((0.223800750674302)*(x37)))+(((5.35649803617952e-6)*(x101)*(x15)))+(((0.288497691800224)*(x13)*(x4)))+(((-0.707111376538080)*(x26)*(x94)))+(((5.35649803617952e-6)*(x1)*(x91)))+(((-0.707111376538080)*(x15)*(x97)))+(((-1.69533162845082e-6)*(x57)))+(((0.288501441627537)*(x0)))+(((-0.223800750674302)*(x38)))+(((-0.707102185784862)*(x114)*(x15)))+(((-0.707102185784862)*(x107)*(x26)))+(((0.223797841800909)*(x69))));
00375 }
00376
00377 IKFAST_API int getNumFreeParameters() { return 1; }
00378 IKFAST_API int* getFreeParameters() { static int freeparams[] = {2}; return freeparams; }
00379 IKFAST_API int getNumJoints() { return 7; }
00380
00381 IKFAST_API int getIKRealSize() { return sizeof(IKReal); }
00382
00383 IKFAST_API int getIKType() { return 0x67000001; }
00384
00385 class IKSolver {
00386 public:
00387 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;
00388
00389 bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
00390 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00391 vsolutions.resize(0); vsolutions.reserve(8);
00392 j2=pfree[0]; cj2=cos(pfree[0]); sj2=sin(pfree[0]);
00393 r00 = eerot[0*3+0];
00394 r01 = eerot[0*3+1];
00395 r02 = eerot[0*3+2];
00396 r10 = eerot[1*3+0];
00397 r11 = eerot[1*3+1];
00398 r12 = eerot[1*3+2];
00399 r20 = eerot[2*3+0];
00400 r21 = eerot[2*3+1];
00401 r22 = eerot[2*3+2];
00402 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00403
00404 new_r00=((((-0.707102185784862)*(r20)))+(((-0.683015503672999)*(r10)))+(((-0.183020000548001)*(r00))));
00405 new_r01=((((-0.707102185784862)*(r21)))+(((-0.683015503672999)*(r11)))+(((-0.183020000548001)*(r01))));
00406 new_r02=((((-0.707102185784862)*(r22)))+(((-0.683015503672999)*(r12)))+(((-0.183020000548001)*(r02))));
00407 new_px=((0.595304777628440)+(((-0.136258591200743)*(r22)))+(((-0.0352679541055998)*(r02)))+(((-0.683015503672999)*(py)))+(((-0.131617087557787)*(r12)))+(((-0.707102185784862)*(pz)))+(((-0.183020000548001)*(px))));
00408 new_r10=((((5.35649803617952e-6)*(r20)))+(((-0.965924897590095)*(r00)))+(((0.258822510973972)*(r10))));
00409 new_r11=((((5.35649803617952e-6)*(r21)))+(((-0.965924897590095)*(r01)))+(((0.258822510973972)*(r11))));
00410 new_r12=((((5.35649803617952e-6)*(r22)))+(((-0.965924897590095)*(r02)))+(((0.258822510973972)*(r12))));
00411 new_py=((0.0399266284442112)+(((5.35649803617952e-6)*(pz)))+(((0.258822510973972)*(py)))+(((-0.186133727765611)*(r02)))+(((0.0498750978646844)*(r12)))+(((-0.965924897590095)*(px)))+(((1.03219717157179e-6)*(r22))));
00412 new_r20=((((0.683008586736249)*(r10)))+(((-0.707111376538080)*(r20)))+(((0.183010304668818)*(r00))));
00413 new_r21=((((0.683008586736249)*(r11)))+(((-0.707111376538080)*(r21)))+(((0.183010304668818)*(r01))));
00414 new_r22=((((0.683008586736249)*(r12)))+(((-0.707111376538080)*(r22)))+(((0.183010304668818)*(r02))));
00415 new_pz=((0.837501372742083)+(((0.183010304668818)*(px)))+(((-0.136260362258888)*(r22)))+(((0.131615754664075)*(r12)))+(((0.0352660857096812)*(r02)))+(((-0.707111376538080)*(pz)))+(((0.683008586736249)*(py))));
00416 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;
00417 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00418 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00419 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00420 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00421 rxp0_0=((((-1.00000000000000)*(py)*(r20)))+(((pz)*(r10))));
00422 rxp0_1=((((px)*(r20)))+(((-1.00000000000000)*(pz)*(r00))));
00423 rxp0_2=((((-1.00000000000000)*(px)*(r10)))+(((py)*(r00))));
00424 rxp1_0=((((-1.00000000000000)*(py)*(r21)))+(((pz)*(r11))));
00425 rxp1_1=((((px)*(r21)))+(((-1.00000000000000)*(pz)*(r01))));
00426 rxp1_2=((((-1.00000000000000)*(px)*(r11)))+(((py)*(r01))));
00427 rxp2_0=((((-1.00000000000000)*(py)*(r22)))+(((pz)*(r12))));
00428 rxp2_1=((((px)*(r22)))+(((-1.00000000000000)*(pz)*(r02))));
00429 rxp2_2=((((-1.00000000000000)*(px)*(r12)))+(((py)*(r02))));
00430 {
00431 IKReal j3array[2], cj3array[2], sj3array[2];
00432 bool j3valid[2]={false};
00433 cj3array[0]=((-1.03241741009200)+(((3.87200693863643)*(pp))));
00434 if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
00435 {
00436 j3valid[0] = j3valid[1] = true;
00437 j3array[0] = IKacos(cj3array[0]);
00438 sj3array[0] = IKsin(j3array[0]);
00439 cj3array[1] = cj3array[0];
00440 j3array[1] = -j3array[0];
00441 sj3array[1] = -sj3array[0];
00442 }
00443 else if( isnan(cj3array[0]) )
00444 {
00445
00446 j3valid[0] = true;
00447 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
00448 }
00449 if( j3valid[0] && j3valid[1] && IKabs(cj3array[0]-cj3array[1]) < 0.0001 && IKabs(sj3array[0]-sj3array[1]) < 0.0001 )
00450 {
00451 j3valid[1]=false;
00452 }
00453 for(int ij3 = 0; ij3 < 2; ++ij3)
00454 {
00455 if( !j3valid[ij3] )
00456 {
00457 continue;
00458 }
00459 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00460
00461 {
00462 IKReal dummyeval[1];
00463 IKReal gconst0;
00464 IKReal x118=(cj2)*(cj2);
00465 IKReal x119=(py)*(py);
00466 IKReal x120=(sj2)*(sj2);
00467 gconst0=IKsign(((((2000.00000000000)*(py)*(((((px)*(py)*(x120)))+(((px)*(py)*(x118)))))))+(((2000.00000000000)*(px)*(((((-1.00000000000000)*(x119)*(x120)))+(((-1.00000000000000)*(x118)*(x119)))))))));
00468 dummyeval[0]=0;
00469 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00470 {
00471 {
00472 IKReal dummyeval[1];
00473 dummyeval[0]=(((px)*(px))+((py)*(py)));
00474 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00475 {
00476 {
00477 IKReal dummyeval[1];
00478 dummyeval[0]=((1.66177758810449)+((cj3)*(cj3))+((((cj2)*(cj2))*((sj3)*(sj3))))+(((2.57819905213270)*(cj3))));
00479 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00480 {
00481 continue;
00482
00483 } else
00484 {
00485 {
00486 IKReal j1array[2], cj1array[2], sj1array[2];
00487 bool j1valid[2]={false};
00488 IKReal x121=((0.316500000000000)*(cj3));
00489 IKReal x122=(cj2)*(cj2);
00490 IKReal x123=(sj3)*(sj3);
00491 IKReal x124=((0.100172250000000)*(x122)*(x123));
00492 IKReal x125=((0.408000000000000)+(x121));
00493 IKReal x126=(x125)*(x125);
00494 IKReal x127=((x126)+(x124));
00495 if( (x127) < (IKReal)-0.00001 )
00496 continue;
00497 IKReal x128=IKsqrt(x127);
00498 IKReal x129=IKabs(x128);
00499 IKReal x130=((IKabs(x129) != 0)?((IKReal)1/(x129)):(IKReal)1.0e30);
00500 IKReal x131=((pz)*(x130));
00501 if( (x131) < -1-IKFAST_SINCOS_THRESH || (x131) > 1+IKFAST_SINCOS_THRESH )
00502 continue;
00503 IKReal x132=IKasin(x131);
00504 IKReal x133=((-0.408000000000000)+(((-1.00000000000000)*(x121))));
00505 IKReal x134=((0.316500000000000)*(cj2)*(sj3));
00506 IKReal x135=IKatan2(x133, x134);
00507 j1array[0]=((x132)+(((-1.00000000000000)*(x135))));
00508 sj1array[0]=IKsin(j1array[0]);
00509 cj1array[0]=IKcos(j1array[0]);
00510 j1array[1]=((3.14159265358979)+(((-1.00000000000000)*(x135)))+(((-1.00000000000000)*(x132))));
00511 sj1array[1]=IKsin(j1array[1]);
00512 cj1array[1]=IKcos(j1array[1]);
00513 if( j1array[0] > IKPI )
00514 {
00515 j1array[0]-=IK2PI;
00516 }
00517 else if( j1array[0] < -IKPI )
00518 { j1array[0]+=IK2PI;
00519 }
00520 j1valid[0] = true;
00521 if( j1array[1] > IKPI )
00522 {
00523 j1array[1]-=IK2PI;
00524 }
00525 else if( j1array[1] < -IKPI )
00526 { j1array[1]+=IK2PI;
00527 }
00528 j1valid[1] = true;
00529 if( j1valid[0] && j1valid[1] && IKabs(cj1array[0]-cj1array[1]) < 0.0001 && IKabs(sj1array[0]-sj1array[1]) < 0.0001 )
00530 {
00531 j1valid[1]=false;
00532 }
00533 for(int ij1 = 0; ij1 < 2; ++ij1)
00534 {
00535 if( !j1valid[ij1] )
00536 {
00537 continue;
00538 }
00539 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00540
00541 {
00542 IKReal dummyeval[1];
00543 IKReal gconst1;
00544 gconst1=IKsign(((((102000.000000000)*(sj1)*((px)*(px))))+(((102000.000000000)*(sj1)*((py)*(py))))));
00545 dummyeval[0]=((((sj1)*((py)*(py))))+(((sj1)*((px)*(px)))));
00546 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00547 {
00548 {
00549 IKReal evalcond[5];
00550 IKReal x136=((0.316500000000000)*(cj3));
00551 IKReal x137=((x136)+(pz));
00552 IKReal x138=((-0.408000000000000)+(((-1.00000000000000)*(x137))));
00553 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j1)), 6.28318530717959)));
00554 evalcond[1]=((0.266636250000000)+(((0.258264000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
00555 evalcond[2]=x138;
00556 evalcond[3]=((-0.0662917500000000)+(((-0.816000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
00557 evalcond[4]=x138;
00558 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 )
00559 {
00560 {
00561 IKReal dummyeval[1];
00562 IKReal gconst2;
00563 gconst2=IKsign(((((-2000.00000000000)*((py)*(py))))+(((-2000.00000000000)*((px)*(px))))));
00564 dummyeval[0]=((((-1.00000000000000)*((px)*(px))))+(((-1.00000000000000)*((py)*(py)))));
00565 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00566 {
00567 continue;
00568
00569 } else
00570 {
00571 {
00572 IKReal j0array[1], cj0array[1], sj0array[1];
00573 bool j0valid[1]={false};
00574 j0array[0]=IKatan2(((gconst2)*(((((633.000000000000)*(cj2)*(py)*(sj3)))+(((-633.000000000000)*(px)*(sj2)*(sj3)))))), ((gconst2)*(((((633.000000000000)*(cj2)*(px)*(sj3)))+(((633.000000000000)*(py)*(sj2)*(sj3)))))));
00575 sj0array[0]=IKsin(j0array[0]);
00576 cj0array[0]=IKcos(j0array[0]);
00577 if( j0array[0] > IKPI )
00578 {
00579 j0array[0]-=IK2PI;
00580 }
00581 else if( j0array[0] < -IKPI )
00582 { j0array[0]+=IK2PI;
00583 }
00584 j0valid[0] = true;
00585 for(int ij0 = 0; ij0 < 1; ++ij0)
00586 {
00587 if( !j0valid[ij0] )
00588 {
00589 continue;
00590 }
00591 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00592
00593 rotationfunction0(vsolutions);
00594 }
00595 }
00596
00597 }
00598
00599 }
00600
00601 } else
00602 {
00603 IKReal x183=((0.316500000000000)*(cj3));
00604 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j1)), 6.28318530717959)));
00605 evalcond[1]=((0.266636250000000)+(((0.258264000000000)*(cj3)))+(((-1.00000000000000)*(pp))));
00606 evalcond[2]=((0.408000000000000)+(x183)+(((-1.00000000000000)*(pz))));
00607 evalcond[3]=((-0.0662917500000000)+(((0.816000000000000)*(pz)))+(((-1.00000000000000)*(pp))));
00608 evalcond[4]=((-0.408000000000000)+(((-1.00000000000000)*(x183)))+(pz));
00609 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 )
00610 {
00611 {
00612 IKReal dummyeval[1];
00613 IKReal gconst3;
00614 gconst3=IKsign(((((2000.00000000000)*((py)*(py))))+(((2000.00000000000)*((px)*(px))))));
00615 dummyeval[0]=(((px)*(px))+((py)*(py)));
00616 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00617 {
00618 continue;
00619
00620 } else
00621 {
00622 {
00623 IKReal j0array[1], cj0array[1], sj0array[1];
00624 bool j0valid[1]={false};
00625 j0array[0]=IKatan2(((gconst3)*(((((633.000000000000)*(cj2)*(py)*(sj3)))+(((633.000000000000)*(px)*(sj2)*(sj3)))))), ((gconst3)*(((((633.000000000000)*(cj2)*(px)*(sj3)))+(((-633.000000000000)*(py)*(sj2)*(sj3)))))));
00626 sj0array[0]=IKsin(j0array[0]);
00627 cj0array[0]=IKcos(j0array[0]);
00628 if( j0array[0] > IKPI )
00629 {
00630 j0array[0]-=IK2PI;
00631 }
00632 else if( j0array[0] < -IKPI )
00633 { j0array[0]+=IK2PI;
00634 }
00635 j0valid[0] = true;
00636 for(int ij0 = 0; ij0 < 1; ++ij0)
00637 {
00638 if( !j0valid[ij0] )
00639 {
00640 continue;
00641 }
00642 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00643
00644 rotationfunction0(vsolutions);
00645 }
00646 }
00647
00648 }
00649
00650 }
00651
00652 } else
00653 {
00654 if( 1 )
00655 {
00656 continue;
00657
00658 } else
00659 {
00660 }
00661 }
00662 }
00663 }
00664
00665 } else
00666 {
00667 {
00668 IKReal j0array[1], cj0array[1], sj0array[1];
00669 bool j0valid[1]={false};
00670 j0array[0]=IKatan2(((gconst1)*(((((-8286.46875000000)*(py)))+(((32283.0000000000)*(px)*(sj1)*(sj2)*(sj3)))+(((-125000.000000000)*(pp)*(py)))+(((-102000.000000000)*(cj1)*(py)*(pz)))))), ((gconst1)*(((((-8286.46875000000)*(px)))+(((-102000.000000000)*(cj1)*(px)*(pz)))+(((-125000.000000000)*(pp)*(px)))+(((-32283.0000000000)*(py)*(sj1)*(sj2)*(sj3)))))));
00671 sj0array[0]=IKsin(j0array[0]);
00672 cj0array[0]=IKcos(j0array[0]);
00673 if( j0array[0] > IKPI )
00674 {
00675 j0array[0]-=IK2PI;
00676 }
00677 else if( j0array[0] < -IKPI )
00678 { j0array[0]+=IK2PI;
00679 }
00680 j0valid[0] = true;
00681 for(int ij0 = 0; ij0 < 1; ++ij0)
00682 {
00683 if( !j0valid[ij0] )
00684 {
00685 continue;
00686 }
00687 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00688
00689 rotationfunction0(vsolutions);
00690 }
00691 }
00692
00693 }
00694
00695 }
00696 }
00697 }
00698
00699 }
00700
00701 }
00702
00703 } else
00704 {
00705 {
00706 IKReal j0array[2], cj0array[2], sj0array[2];
00707 bool j0valid[2]={false};
00708 IKReal x184=((-1.00000000000000)*(py));
00709 IKReal x185=IKatan2(x184, px);
00710 IKReal x186=(px)*(px);
00711 IKReal x187=(py)*(py);
00712 IKReal x188=((x186)+(x187));
00713 if( (x188) < (IKReal)-0.00001 )
00714 continue;
00715 IKReal x189=IKsqrt(x188);
00716 IKReal x190=IKabs(x189);
00717 IKReal x191=((IKabs(x190) != 0)?((IKReal)1/(x190)):(IKReal)1.0e30);
00718 IKReal x192=((0.316500000000000)*(sj2)*(sj3)*(x191));
00719 if( (x192) < -1-IKFAST_SINCOS_THRESH || (x192) > 1+IKFAST_SINCOS_THRESH )
00720 continue;
00721 IKReal x193=IKasin(x192);
00722 j0array[0]=((((-1.00000000000000)*(x185)))+(x193));
00723 sj0array[0]=IKsin(j0array[0]);
00724 cj0array[0]=IKcos(j0array[0]);
00725 j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x185)))+(((-1.00000000000000)*(x193))));
00726 sj0array[1]=IKsin(j0array[1]);
00727 cj0array[1]=IKcos(j0array[1]);
00728 if( j0array[0] > IKPI )
00729 {
00730 j0array[0]-=IK2PI;
00731 }
00732 else if( j0array[0] < -IKPI )
00733 { j0array[0]+=IK2PI;
00734 }
00735 j0valid[0] = true;
00736 if( j0array[1] > IKPI )
00737 {
00738 j0array[1]-=IK2PI;
00739 }
00740 else if( j0array[1] < -IKPI )
00741 { j0array[1]+=IK2PI;
00742 }
00743 j0valid[1] = true;
00744 if( j0valid[0] && j0valid[1] && IKabs(cj0array[0]-cj0array[1]) < 0.0001 && IKabs(sj0array[0]-sj0array[1]) < 0.0001 )
00745 {
00746 j0valid[1]=false;
00747 }
00748 for(int ij0 = 0; ij0 < 2; ++ij0)
00749 {
00750 if( !j0valid[ij0] )
00751 {
00752 continue;
00753 }
00754 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00755
00756 {
00757 IKReal dummyeval[1];
00758 IKReal gconst4;
00759 gconst4=IKsign(((-665856.000000000)+(((-400689.000000000)*((cj2)*(cj2))*((sj3)*(sj3))))+(((-400689.000000000)*((cj3)*(cj3))))+(((-1033056.00000000)*(cj3)))));
00760 dummyeval[0]=((-1.66177758810449)+(((-2.57819905213270)*(cj3)))+(((-1.00000000000000)*((cj2)*(cj2))*((sj3)*(sj3))))+(((-1.00000000000000)*((cj3)*(cj3)))));
00761 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00762 {
00763 continue;
00764
00765 } else
00766 {
00767 {
00768 IKReal j1array[1], cj1array[1], sj1array[1];
00769 bool j1valid[1]={false};
00770 j1array[0]=IKatan2(((gconst4)*(((((-1266000.00000000)*(cj2)*(pz)*(sj3)))+(((1632000.00000000)*(py)*(sj0)))+(((1632000.00000000)*(cj0)*(px)))+(((1266000.00000000)*(cj3)*(py)*(sj0)))+(((1266000.00000000)*(cj0)*(cj3)*(px)))))), ((gconst4)*(((((1266000.00000000)*(cj3)*(pz)))+(((1266000.00000000)*(cj0)*(cj2)*(px)*(sj3)))+(((1632000.00000000)*(pz)))+(((1266000.00000000)*(cj2)*(py)*(sj0)*(sj3)))))));
00771 sj1array[0]=IKsin(j1array[0]);
00772 cj1array[0]=IKcos(j1array[0]);
00773 if( j1array[0] > IKPI )
00774 {
00775 j1array[0]-=IK2PI;
00776 }
00777 else if( j1array[0] < -IKPI )
00778 { j1array[0]+=IK2PI;
00779 }
00780 j1valid[0] = true;
00781 for(int ij1 = 0; ij1 < 1; ++ij1)
00782 {
00783 if( !j1valid[ij1] )
00784 {
00785 continue;
00786 }
00787 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00788
00789 rotationfunction0(vsolutions);
00790 }
00791 }
00792
00793 }
00794
00795 }
00796 }
00797 }
00798
00799 }
00800
00801 }
00802
00803 } else
00804 {
00805 {
00806 IKReal j0array[1], cj0array[1], sj0array[1];
00807 bool j0valid[1]={false};
00808 IKReal x194=(py)*(py);
00809 IKReal x195=(sj2)*(sj2);
00810 IKReal x196=(cj2)*(cj2);
00811 j0array[0]=IKatan2(((gconst0)*(((((633.000000000000)*(sj2)*(sj3)*(x194)))+(((633.000000000000)*(sj2)*(sj3)*(((((-1.00000000000000)*(x194)*(x196)))+(((-1.00000000000000)*(x194)*(x195)))))))))), ((gconst0)*(((((-633.000000000000)*(sj2)*(sj3)*(((((px)*(py)*(x195)))+(((px)*(py)*(x196)))))))+(((633.000000000000)*(px)*(py)*(sj2)*(sj3)))))));
00812 sj0array[0]=IKsin(j0array[0]);
00813 cj0array[0]=IKcos(j0array[0]);
00814 if( j0array[0] > IKPI )
00815 {
00816 j0array[0]-=IK2PI;
00817 }
00818 else if( j0array[0] < -IKPI )
00819 { j0array[0]+=IK2PI;
00820 }
00821 j0valid[0] = true;
00822 for(int ij0 = 0; ij0 < 1; ++ij0)
00823 {
00824 if( !j0valid[ij0] )
00825 {
00826 continue;
00827 }
00828 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00829
00830 {
00831 IKReal dummyeval[1];
00832 IKReal gconst4;
00833 gconst4=IKsign(((-665856.000000000)+(((-400689.000000000)*((cj2)*(cj2))*((sj3)*(sj3))))+(((-400689.000000000)*((cj3)*(cj3))))+(((-1033056.00000000)*(cj3)))));
00834 dummyeval[0]=((-1.66177758810449)+(((-2.57819905213270)*(cj3)))+(((-1.00000000000000)*((cj2)*(cj2))*((sj3)*(sj3))))+(((-1.00000000000000)*((cj3)*(cj3)))));
00835 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00836 {
00837 continue;
00838
00839 } else
00840 {
00841 {
00842 IKReal j1array[1], cj1array[1], sj1array[1];
00843 bool j1valid[1]={false};
00844 j1array[0]=IKatan2(((gconst4)*(((((-1266000.00000000)*(cj2)*(pz)*(sj3)))+(((1632000.00000000)*(py)*(sj0)))+(((1632000.00000000)*(cj0)*(px)))+(((1266000.00000000)*(cj3)*(py)*(sj0)))+(((1266000.00000000)*(cj0)*(cj3)*(px)))))), ((gconst4)*(((((1266000.00000000)*(cj3)*(pz)))+(((1266000.00000000)*(cj0)*(cj2)*(px)*(sj3)))+(((1632000.00000000)*(pz)))+(((1266000.00000000)*(cj2)*(py)*(sj0)*(sj3)))))));
00845 sj1array[0]=IKsin(j1array[0]);
00846 cj1array[0]=IKcos(j1array[0]);
00847 if( j1array[0] > IKPI )
00848 {
00849 j1array[0]-=IK2PI;
00850 }
00851 else if( j1array[0] < -IKPI )
00852 { j1array[0]+=IK2PI;
00853 }
00854 j1valid[0] = true;
00855 for(int ij1 = 0; ij1 < 1; ++ij1)
00856 {
00857 if( !j1valid[ij1] )
00858 {
00859 continue;
00860 }
00861 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00862
00863 rotationfunction0(vsolutions);
00864 }
00865 }
00866
00867 }
00868
00869 }
00870 }
00871 }
00872
00873 }
00874
00875 }
00876 }
00877 }
00878 }
00879 return vsolutions.size()>0;
00880 }
00881 inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
00882 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
00883 IKReal x139=((sj1)*(sj3));
00884 IKReal x140=((cj1)*(cj2)*(cj3));
00885 IKReal x141=((((-1.00000000000000)*(x140)))+(x139));
00886 IKReal x142=((cj1)*(sj3));
00887 IKReal x143=((cj2)*(cj3)*(sj1));
00888 IKReal x144=((x142)+(x143));
00889 IKReal x145=((sj0)*(x141));
00890 IKReal x146=((cj0)*(cj3)*(sj2));
00891 IKReal x147=((((-1.00000000000000)*(x146)))+(x145));
00892 IKReal x148=((cj0)*(x141));
00893 IKReal x149=((cj3)*(sj0)*(sj2));
00894 IKReal x150=((x148)+(x149));
00895 IKReal x151=((cj2)*(sj0));
00896 IKReal x152=((cj0)*(cj1)*(sj2));
00897 IKReal x153=((x152)+(x151));
00898 IKReal x154=((cj1)*(sj0)*(sj2));
00899 IKReal x155=((cj0)*(cj2));
00900 IKReal x156=((((-1.00000000000000)*(x155)))+(x154));
00901 IKReal x157=((cj2)*(x142));
00902 IKReal x158=((cj3)*(sj1));
00903 IKReal x159=((x157)+(x158));
00904 IKReal x160=((cj1)*(cj3));
00905 IKReal x161=((cj2)*(x139));
00906 IKReal x162=((((-1.00000000000000)*(x161)))+(x160));
00907 IKReal x163=((sj0)*(x159));
00908 IKReal x164=((cj0)*(sj2)*(sj3));
00909 IKReal x165=((x163)+(x164));
00910 IKReal x166=((cj0)*(x159));
00911 IKReal x167=((sj0)*(sj2)*(sj3));
00912 IKReal x168=((((-1.00000000000000)*(x167)))+(x166));
00913 new_r00=((((r20)*(x144)))+(((r00)*(x150)))+(((r10)*(x147))));
00914 new_r01=((((r01)*(x150)))+(((r21)*(x144)))+(((r11)*(x147))));
00915 new_r02=((((r02)*(x150)))+(((r22)*(x144)))+(((r12)*(x147))));
00916 new_r10=((((r00)*(x153)))+(((-1.00000000000000)*(r20)*(sj1)*(sj2)))+(((r10)*(x156))));
00917 new_r11=((((r01)*(x153)))+(((-1.00000000000000)*(r21)*(sj1)*(sj2)))+(((r11)*(x156))));
00918 new_r12=((((-1.00000000000000)*(r22)*(sj1)*(sj2)))+(((r02)*(x153)))+(((r12)*(x156))));
00919 new_r20=((((r00)*(x168)))+(((r20)*(x162)))+(((r10)*(x165))));
00920 new_r21=((((r01)*(x168)))+(((r21)*(x162)))+(((r11)*(x165))));
00921 new_r22=((((r22)*(x162)))+(((r12)*(x165)))+(((r02)*(x168))));
00922 {
00923 IKReal j5array[2], cj5array[2], sj5array[2];
00924 bool j5valid[2]={false};
00925 cj5array[0]=new_r22;
00926 if( cj5array[0] >= -1-IKFAST_SINCOS_THRESH && cj5array[0] <= 1+IKFAST_SINCOS_THRESH )
00927 {
00928 j5valid[0] = j5valid[1] = true;
00929 j5array[0] = IKacos(cj5array[0]);
00930 sj5array[0] = IKsin(j5array[0]);
00931 cj5array[1] = cj5array[0];
00932 j5array[1] = -j5array[0];
00933 sj5array[1] = -sj5array[0];
00934 }
00935 else if( isnan(cj5array[0]) )
00936 {
00937
00938 j5valid[0] = true;
00939 cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
00940 }
00941 if( j5valid[0] && j5valid[1] && IKabs(cj5array[0]-cj5array[1]) < 0.0001 && IKabs(sj5array[0]-sj5array[1]) < 0.0001 )
00942 {
00943 j5valid[1]=false;
00944 }
00945 for(int ij5 = 0; ij5 < 2; ++ij5)
00946 {
00947 if( !j5valid[ij5] )
00948 {
00949 continue;
00950 }
00951 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00952
00953 {
00954 IKReal dummyeval[1];
00955 IKReal gconst6;
00956 gconst6=IKsign(sj5);
00957 dummyeval[0]=sj5;
00958 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00959 {
00960 {
00961 IKReal dummyeval[1];
00962 IKReal gconst5;
00963 gconst5=IKsign(((((-1.00000000000000)*((new_r02)*(new_r02))))+(((-1.00000000000000)*((new_r12)*(new_r12))))));
00964 dummyeval[0]=((((-1.00000000000000)*((new_r02)*(new_r02))))+(((-1.00000000000000)*((new_r12)*(new_r12)))));
00965 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00966 {
00967 {
00968 IKReal evalcond[7];
00969 IKReal x169=((-1.00000000000000)+(new_r22));
00970 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j5)), 6.28318530717959)));
00971 evalcond[1]=x169;
00972 evalcond[2]=new_r21;
00973 evalcond[3]=new_r20;
00974 evalcond[4]=new_r20;
00975 evalcond[5]=new_r21;
00976 evalcond[6]=x169;
00977 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 )
00978 {
00979 {
00980 IKReal j4array[2], cj4array[2], sj4array[2];
00981 bool j4valid[2]={false};
00982 IKReal x170=IKatan2(new_r02, new_r12);
00983 j4array[0]=((-1.00000000000000)*(x170));
00984 sj4array[0]=IKsin(j4array[0]);
00985 cj4array[0]=IKcos(j4array[0]);
00986 j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x170))));
00987 sj4array[1]=IKsin(j4array[1]);
00988 cj4array[1]=IKcos(j4array[1]);
00989 if( j4array[0] > IKPI )
00990 {
00991 j4array[0]-=IK2PI;
00992 }
00993 else if( j4array[0] < -IKPI )
00994 { j4array[0]+=IK2PI;
00995 }
00996 j4valid[0] = true;
00997 if( j4array[1] > IKPI )
00998 {
00999 j4array[1]-=IK2PI;
01000 }
01001 else if( j4array[1] < -IKPI )
01002 { j4array[1]+=IK2PI;
01003 }
01004 j4valid[1] = true;
01005 if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
01006 {
01007 j4valid[1]=false;
01008 }
01009 for(int ij4 = 0; ij4 < 2; ++ij4)
01010 {
01011 if( !j4valid[ij4] )
01012 {
01013 continue;
01014 }
01015 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01016
01017 {
01018 IKReal j6array[1], cj6array[1], sj6array[1];
01019 bool j6valid[1]={false};
01020 j6array[0]=IKatan2(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))), ((((cj4)*(new_r11)))+(((-1.00000000000000)*(new_r01)*(sj4)))));
01021 sj6array[0]=IKsin(j6array[0]);
01022 cj6array[0]=IKcos(j6array[0]);
01023 if( j6array[0] > IKPI )
01024 {
01025 j6array[0]-=IK2PI;
01026 }
01027 else if( j6array[0] < -IKPI )
01028 { j6array[0]+=IK2PI;
01029 }
01030 j6valid[0] = true;
01031 for(int ij6 = 0; ij6 < 1; ++ij6)
01032 {
01033 if( !j6valid[ij6] )
01034 {
01035 continue;
01036 }
01037 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01038
01039 {
01040 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01041 solution.basesol.resize(7);
01042 solution.basesol[0].foffset = j0;
01043 solution.basesol[1].foffset = j1;
01044 solution.basesol[2].foffset = j2;
01045 solution.basesol[3].foffset = j3;
01046 solution.basesol[4].foffset = j4;
01047 solution.basesol[5].foffset = j5;
01048 solution.basesol[6].foffset = j6;
01049 solution.vfree.resize(0);
01050 }
01051 }
01052 }
01053 }
01054 }
01055
01056 } else
01057 {
01058 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
01059 evalcond[1]=((1.00000000000000)+(new_r22));
01060 evalcond[2]=new_r21;
01061 evalcond[3]=new_r20;
01062 evalcond[4]=((-1.00000000000000)*(new_r20));
01063 evalcond[5]=((-1.00000000000000)*(new_r21));
01064 evalcond[6]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
01065 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 )
01066 {
01067 {
01068 IKReal j4array[2], cj4array[2], sj4array[2];
01069 bool j4valid[2]={false};
01070 IKReal x171=IKatan2(new_r02, new_r12);
01071 j4array[0]=((-1.00000000000000)*(x171));
01072 sj4array[0]=IKsin(j4array[0]);
01073 cj4array[0]=IKcos(j4array[0]);
01074 j4array[1]=((3.14159265358979)+(((-1.00000000000000)*(x171))));
01075 sj4array[1]=IKsin(j4array[1]);
01076 cj4array[1]=IKcos(j4array[1]);
01077 if( j4array[0] > IKPI )
01078 {
01079 j4array[0]-=IK2PI;
01080 }
01081 else if( j4array[0] < -IKPI )
01082 { j4array[0]+=IK2PI;
01083 }
01084 j4valid[0] = true;
01085 if( j4array[1] > IKPI )
01086 {
01087 j4array[1]-=IK2PI;
01088 }
01089 else if( j4array[1] < -IKPI )
01090 { j4array[1]+=IK2PI;
01091 }
01092 j4valid[1] = true;
01093 if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
01094 {
01095 j4valid[1]=false;
01096 }
01097 for(int ij4 = 0; ij4 < 2; ++ij4)
01098 {
01099 if( !j4valid[ij4] )
01100 {
01101 continue;
01102 }
01103 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01104
01105 {
01106 IKReal j6array[1], cj6array[1], sj6array[1];
01107 bool j6valid[1]={false};
01108 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((-1.00000000000000)*(new_r00)*(sj4)))), ((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))));
01109 sj6array[0]=IKsin(j6array[0]);
01110 cj6array[0]=IKcos(j6array[0]);
01111 if( j6array[0] > IKPI )
01112 {
01113 j6array[0]-=IK2PI;
01114 }
01115 else if( j6array[0] < -IKPI )
01116 { j6array[0]+=IK2PI;
01117 }
01118 j6valid[0] = true;
01119 for(int ij6 = 0; ij6 < 1; ++ij6)
01120 {
01121 if( !j6valid[ij6] )
01122 {
01123 continue;
01124 }
01125 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01126
01127 {
01128 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01129 solution.basesol.resize(7);
01130 solution.basesol[0].foffset = j0;
01131 solution.basesol[1].foffset = j1;
01132 solution.basesol[2].foffset = j2;
01133 solution.basesol[3].foffset = j3;
01134 solution.basesol[4].foffset = j4;
01135 solution.basesol[5].foffset = j5;
01136 solution.basesol[6].foffset = j6;
01137 solution.vfree.resize(0);
01138 }
01139 }
01140 }
01141 }
01142 }
01143
01144 } else
01145 {
01146 if( 1 )
01147 {
01148 continue;
01149
01150 } else
01151 {
01152 }
01153 }
01154 }
01155 }
01156
01157 } else
01158 {
01159 {
01160 IKReal j4array[1], cj4array[1], sj4array[1];
01161 bool j4valid[1]={false};
01162 j4array[0]=IKatan2(((gconst5)*(new_r12)*(sj5)), ((gconst5)*(new_r02)*(sj5)));
01163 sj4array[0]=IKsin(j4array[0]);
01164 cj4array[0]=IKcos(j4array[0]);
01165 if( j4array[0] > IKPI )
01166 {
01167 j4array[0]-=IK2PI;
01168 }
01169 else if( j4array[0] < -IKPI )
01170 { j4array[0]+=IK2PI;
01171 }
01172 j4valid[0] = true;
01173 for(int ij4 = 0; ij4 < 1; ++ij4)
01174 {
01175 if( !j4valid[ij4] )
01176 {
01177 continue;
01178 }
01179 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01180
01181 {
01182 IKReal dummyeval[1];
01183 IKReal gconst7;
01184 gconst7=IKsign(sj5);
01185 dummyeval[0]=sj5;
01186 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01187 {
01188 {
01189 IKReal evalcond[11];
01190 IKReal x172=((new_r02)*(sj4));
01191 IKReal x173=((cj4)*(new_r12));
01192 IKReal x174=((new_r12)*(sj4));
01193 IKReal x175=((cj4)*(new_r02));
01194 IKReal x176=((x175)+(x174));
01195 IKReal x177=((-1.00000000000000)+(new_r22));
01196 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j5)), 6.28318530717959)));
01197 evalcond[1]=x177;
01198 evalcond[2]=new_r21;
01199 evalcond[3]=new_r20;
01200 evalcond[4]=((((-1.00000000000000)*(x172)))+(x173));
01201 evalcond[5]=((((-1.00000000000000)*(x173)))+(x172));
01202 evalcond[6]=x176;
01203 evalcond[7]=x176;
01204 evalcond[8]=new_r20;
01205 evalcond[9]=new_r21;
01206 evalcond[10]=x177;
01207 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 )
01208 {
01209 {
01210 IKReal j6array[1], cj6array[1], sj6array[1];
01211 bool j6valid[1]={false};
01212 j6array[0]=IKatan2(((((-1.00000000000000)*(cj4)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj4)))), ((((cj4)*(new_r11)))+(((-1.00000000000000)*(new_r01)*(sj4)))));
01213 sj6array[0]=IKsin(j6array[0]);
01214 cj6array[0]=IKcos(j6array[0]);
01215 if( j6array[0] > IKPI )
01216 {
01217 j6array[0]-=IK2PI;
01218 }
01219 else if( j6array[0] < -IKPI )
01220 { j6array[0]+=IK2PI;
01221 }
01222 j6valid[0] = true;
01223 for(int ij6 = 0; ij6 < 1; ++ij6)
01224 {
01225 if( !j6valid[ij6] )
01226 {
01227 continue;
01228 }
01229 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01230
01231 {
01232 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01233 solution.basesol.resize(7);
01234 solution.basesol[0].foffset = j0;
01235 solution.basesol[1].foffset = j1;
01236 solution.basesol[2].foffset = j2;
01237 solution.basesol[3].foffset = j3;
01238 solution.basesol[4].foffset = j4;
01239 solution.basesol[5].foffset = j5;
01240 solution.basesol[6].foffset = j6;
01241 solution.vfree.resize(0);
01242 }
01243 }
01244 }
01245
01246 } else
01247 {
01248 IKReal x178=((new_r02)*(sj4));
01249 IKReal x179=((cj4)*(new_r12));
01250 IKReal x180=((new_r12)*(sj4));
01251 IKReal x181=((cj4)*(new_r02));
01252 IKReal x182=((x180)+(x181));
01253 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j5)), 6.28318530717959)));
01254 evalcond[1]=((1.00000000000000)+(new_r22));
01255 evalcond[2]=new_r21;
01256 evalcond[3]=new_r20;
01257 evalcond[4]=((((-1.00000000000000)*(x178)))+(x179));
01258 evalcond[5]=((((-1.00000000000000)*(x179)))+(x178));
01259 evalcond[6]=x182;
01260 evalcond[7]=((-1.00000000000000)*(x182));
01261 evalcond[8]=((-1.00000000000000)*(new_r20));
01262 evalcond[9]=((-1.00000000000000)*(new_r21));
01263 evalcond[10]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
01264 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 )
01265 {
01266 {
01267 IKReal j6array[1], cj6array[1], sj6array[1];
01268 bool j6valid[1]={false};
01269 j6array[0]=IKatan2(((((cj4)*(new_r10)))+(((-1.00000000000000)*(new_r00)*(sj4)))), ((((-1.00000000000000)*(new_r10)*(sj4)))+(((-1.00000000000000)*(cj4)*(new_r00)))));
01270 sj6array[0]=IKsin(j6array[0]);
01271 cj6array[0]=IKcos(j6array[0]);
01272 if( j6array[0] > IKPI )
01273 {
01274 j6array[0]-=IK2PI;
01275 }
01276 else if( j6array[0] < -IKPI )
01277 { j6array[0]+=IK2PI;
01278 }
01279 j6valid[0] = true;
01280 for(int ij6 = 0; ij6 < 1; ++ij6)
01281 {
01282 if( !j6valid[ij6] )
01283 {
01284 continue;
01285 }
01286 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01287
01288 {
01289 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01290 solution.basesol.resize(7);
01291 solution.basesol[0].foffset = j0;
01292 solution.basesol[1].foffset = j1;
01293 solution.basesol[2].foffset = j2;
01294 solution.basesol[3].foffset = j3;
01295 solution.basesol[4].foffset = j4;
01296 solution.basesol[5].foffset = j5;
01297 solution.basesol[6].foffset = j6;
01298 solution.vfree.resize(0);
01299 }
01300 }
01301 }
01302
01303 } else
01304 {
01305 if( 1 )
01306 {
01307 continue;
01308
01309 } else
01310 {
01311 }
01312 }
01313 }
01314 }
01315
01316 } else
01317 {
01318 {
01319 IKReal j6array[1], cj6array[1], sj6array[1];
01320 bool j6valid[1]={false};
01321 j6array[0]=IKatan2(((-1.00000000000000)*(gconst7)*(new_r21)), ((gconst7)*(new_r20)));
01322 sj6array[0]=IKsin(j6array[0]);
01323 cj6array[0]=IKcos(j6array[0]);
01324 if( j6array[0] > IKPI )
01325 {
01326 j6array[0]-=IK2PI;
01327 }
01328 else if( j6array[0] < -IKPI )
01329 { j6array[0]+=IK2PI;
01330 }
01331 j6valid[0] = true;
01332 for(int ij6 = 0; ij6 < 1; ++ij6)
01333 {
01334 if( !j6valid[ij6] )
01335 {
01336 continue;
01337 }
01338 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01339
01340 {
01341 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01342 solution.basesol.resize(7);
01343 solution.basesol[0].foffset = j0;
01344 solution.basesol[1].foffset = j1;
01345 solution.basesol[2].foffset = j2;
01346 solution.basesol[3].foffset = j3;
01347 solution.basesol[4].foffset = j4;
01348 solution.basesol[5].foffset = j5;
01349 solution.basesol[6].foffset = j6;
01350 solution.vfree.resize(0);
01351 }
01352 }
01353 }
01354
01355 }
01356
01357 }
01358 }
01359 }
01360
01361 }
01362
01363 }
01364
01365 } else
01366 {
01367 {
01368 IKReal j6array[1], cj6array[1], sj6array[1];
01369 bool j6valid[1]={false};
01370 j6array[0]=IKatan2(((-1.00000000000000)*(gconst6)*(new_r21)), ((gconst6)*(new_r20)));
01371 sj6array[0]=IKsin(j6array[0]);
01372 cj6array[0]=IKcos(j6array[0]);
01373 if( j6array[0] > IKPI )
01374 {
01375 j6array[0]-=IK2PI;
01376 }
01377 else if( j6array[0] < -IKPI )
01378 { j6array[0]+=IK2PI;
01379 }
01380 j6valid[0] = true;
01381 for(int ij6 = 0; ij6 < 1; ++ij6)
01382 {
01383 if( !j6valid[ij6] )
01384 {
01385 continue;
01386 }
01387 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
01388
01389 {
01390 IKReal dummyeval[1];
01391 IKReal gconst8;
01392 gconst8=IKsign(((((-1.00000000000000)*((new_r02)*(new_r02))))+(((-1.00000000000000)*((new_r12)*(new_r12))))));
01393 dummyeval[0]=((((-1.00000000000000)*((new_r02)*(new_r02))))+(((-1.00000000000000)*((new_r12)*(new_r12)))));
01394 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
01395 {
01396 continue;
01397
01398 } else
01399 {
01400 {
01401 IKReal j4array[1], cj4array[1], sj4array[1];
01402 bool j4valid[1]={false};
01403 j4array[0]=IKatan2(((gconst8)*(new_r12)*(sj5)), ((gconst8)*(new_r02)*(sj5)));
01404 sj4array[0]=IKsin(j4array[0]);
01405 cj4array[0]=IKcos(j4array[0]);
01406 if( j4array[0] > IKPI )
01407 {
01408 j4array[0]-=IK2PI;
01409 }
01410 else if( j4array[0] < -IKPI )
01411 { j4array[0]+=IK2PI;
01412 }
01413 j4valid[0] = true;
01414 for(int ij4 = 0; ij4 < 1; ++ij4)
01415 {
01416 if( !j4valid[ij4] )
01417 {
01418 continue;
01419 }
01420 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
01421
01422 {
01423 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01424 solution.basesol.resize(7);
01425 solution.basesol[0].foffset = j0;
01426 solution.basesol[1].foffset = j1;
01427 solution.basesol[2].foffset = j2;
01428 solution.basesol[3].foffset = j3;
01429 solution.basesol[4].foffset = j4;
01430 solution.basesol[5].foffset = j5;
01431 solution.basesol[6].foffset = j6;
01432 solution.vfree.resize(0);
01433 }
01434 }
01435 }
01436
01437 }
01438
01439 }
01440 }
01441 }
01442
01443 }
01444
01445 }
01446 }
01447 }
01448 }
01449 }};
01450
01451
01454 IKFAST_API bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
01455 IKSolver solver;
01456 return solver.ik(eetrans,eerot,pfree,vsolutions);
01457 }
01458
01459 IKFAST_API const char* getKinematicsHash() { return "<robot:genericrobot - cob3-2 (aca6a0c661b045f7da1c37f82c4bf10f)>"; }
01460
01461 #ifdef IKFAST_NAMESPACE
01462 }
01463 #endif
01464
01465 #ifndef IKFAST_NO_MAIN
01466 #include <stdio.h>
01467 #include <stdlib.h>
01468 #ifdef IKFAST_NAMESPACE
01469 using namespace IKFAST_NAMESPACE;
01470 #endif
01471 int main(int argc, char** argv)
01472 {
01473 if( argc != 12+getNumFreeParameters()+1 ) {
01474 printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
01475 "Returns the ik solutions given the transformation of the end effector specified by\n"
01476 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
01477 "There are %d free parameters that have to be specified.\n\n",getNumFreeParameters());
01478 return 1;
01479 }
01480
01481 std::vector<IKSolution> vsolutions;
01482 std::vector<IKReal> vfree(getNumFreeParameters());
01483 IKReal eerot[9],eetrans[3];
01484 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
01485 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
01486 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
01487 for(std::size_t i = 0; i < vfree.size(); ++i)
01488 vfree[i] = atof(argv[13+i]);
01489 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
01490
01491 if( !bSuccess ) {
01492 fprintf(stderr,"Failed to get ik solution\n");
01493 return -1;
01494 }
01495
01496 printf("Found %d ik solutions:\n", (int)vsolutions.size());
01497 std::vector<IKReal> sol(getNumJoints());
01498 for(std::size_t i = 0; i < vsolutions.size(); ++i) {
01499 printf("sol%d (free=%d): ", (int)i, (int)vsolutions[i].GetFree().size());
01500 std::vector<IKReal> vsolfree(vsolutions[i].GetFree().size());
01501 vsolutions[i].GetSolution(&sol[0],vsolfree.size()>0?&vsolfree[0]:NULL);
01502 for( std::size_t j = 0; j < sol.size(); ++j)
01503 printf("%.15f, ", sol[j]);
01504 printf("\n");
01505 }
01506 return 0;
01507 }
01508
01509 #endif
