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