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;
00245 x0=IKcos(j[0]);
00246 x1=IKcos(j[1]);
00247 x2=IKsin(j[2]);
00248 x3=IKcos(j[2]);
00249 x4=IKsin(j[1]);
00250 x5=IKcos(j[4]);
00251 x6=IKcos(j[3]);
00252 x7=((x0)*(x1)*(x2));
00253 x8=((x0)*(x3)*(x4));
00254 x9=((x7)+(x8));
00255 x10=((-1.00000000000000)*(x9));
00256 x11=((x10)*(x6));
00257 x12=IKsin(j[0]);
00258 x13=IKsin(j[3]);
00259 x14=((x12)*(x13));
00260 x15=((((-1.00000000000000)*(x14)))+(x11));
00261 x16=IKsin(j[4]);
00262 x17=((x0)*(x2)*(x4));
00263 x18=((x0)*(x1)*(x3));
00264 x19=IKcos(j[5]);
00265 x20=((x15)*(x5));
00266 x21=((((-1.00000000000000)*(x18)))+(x17));
00267 x22=((x16)*(x21));
00268 x23=((x20)+(x22));
00269 x24=((-1.00000000000000)*(x23));
00270 x25=IKsin(j[5]);
00271 x26=((x12)*(x6));
00272 x27=((x13)*(x9));
00273 x28=((x1)*(x12)*(x2));
00274 x29=((x12)*(x3)*(x4));
00275 x30=((x28)+(x29));
00276 x31=((x12)*(x2)*(x4));
00277 x32=((x1)*(x12)*(x3));
00278 x33=((x0)*(x13));
00279 x34=((-1.00000000000000)*(x30));
00280 x35=((x34)*(x6));
00281 x36=((x33)+(x35));
00282 x37=((x13)*(x30));
00283 x38=((x0)*(x6));
00284 x39=((x38)+(x37));
00285 x40=((((-1.00000000000000)*(x32)))+(x31));
00286 x41=((x16)*(x40));
00287 x42=((x36)*(x5));
00288 x43=((x42)+(x41));
00289 x44=((-1.00000000000000)*(x43));
00290 x45=((x1)*(x2));
00291 x46=((x3)*(x4));
00292 x47=((x46)+(x45));
00293 x48=((x2)*(x4));
00294 x49=((x1)*(x3));
00295 x50=((((-1.00000000000000)*(x49)))+(x48));
00296 eerot[0]=((((x5)*(((((-1.00000000000000)*(x17)))+(x18)))))+(((x15)*(x16))));
00297 eerot[1]=((((x19)*(((((-1.00000000000000)*(x26)))+(x27)))))+(((x24)*(x25))));
00298 eerot[2]=((((x19)*(x24)))+(((x25)*(((((-1.00000000000000)*(x27)))+(x26))))));
00299 eetrans[0]=((((0.270000000000000)*(x0)*(x4)))+(((x5)*(((((0.0800000000000000)*(x18)))+(((-0.0800000000000000)*(x17)))))))+(((0.295000000000000)*(x18)))+(((0.0740000000000000)*(x0)))+(((-0.295000000000000)*(x17)))+(((0.0900000000000000)*(x7)))+(((0.0900000000000000)*(x8)))+(((x16)*(((((0.0800000000000000)*(x11)))+(((-0.0800000000000000)*(x14))))))));
00300 eerot[3]=((((x5)*(((((-1.00000000000000)*(x31)))+(x32)))))+(((x16)*(x36))));
00301 eerot[4]=((((x19)*(x39)))+(((x25)*(x44))));
00302 eerot[5]=((((x19)*(x44)))+(((-1.00000000000000)*(x25)*(x39))));
00303 eetrans[1]=((((0.295000000000000)*(x32)))+(((-0.295000000000000)*(x31)))+(((0.0900000000000000)*(x29)))+(((0.0900000000000000)*(x28)))+(((0.270000000000000)*(x12)*(x4)))+(((x16)*(((((0.0800000000000000)*(x35)))+(((0.0800000000000000)*(x33)))))))+(((0.0740000000000000)*(x12)))+(((x5)*(((((0.0800000000000000)*(x32)))+(((-0.0800000000000000)*(x31))))))));
00304 eerot[6]=((((-1.00000000000000)*(x47)*(x5)))+(((x16)*(x50)*(x6))));
00305 eerot[7]=((((x13)*(x19)*(((((-1.00000000000000)*(x48)))+(x49)))))+(((x25)*(((((-1.00000000000000)*(x16)*(x47)))+(((-1.00000000000000)*(x5)*(x50)*(x6))))))));
00306 eerot[8]=((((x19)*(((((-1.00000000000000)*(x16)*(x47)))+(((-1.00000000000000)*(x5)*(x50)*(x6)))))))+(((x13)*(x25)*(x50))));
00307 eetrans[2]=((0.390950000000000)+(((x5)*(((((-0.0800000000000000)*(x46)))+(((-0.0800000000000000)*(x45)))))))+(((x16)*(x6)*(((((0.0800000000000000)*(x48)))+(((-0.0800000000000000)*(x49)))))))+(((-0.295000000000000)*(x46)))+(((-0.295000000000000)*(x45)))+(((0.0900000000000000)*(x49)))+(((-0.0900000000000000)*(x48)))+(((0.270000000000000)*(x1))));
00308 }
00309
00310 IKFAST_API int getNumFreeParameters() { return 0; }
00311 IKFAST_API int* getFreeParameters() { return NULL; }
00312 IKFAST_API int getNumJoints() { return 6; }
00313
00314 IKFAST_API int getIKRealSize() { return sizeof(IKReal); }
00315
00316 IKFAST_API int getIKType() { return 0x67000001; }
00317
00318 class IKSolver {
00319 public:
00320 IKReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,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;
00321
00322 bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
00323 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
00324 vsolutions.resize(0); vsolutions.reserve(8);
00325 r00 = eerot[0*3+0];
00326 r01 = eerot[0*3+1];
00327 r02 = eerot[0*3+2];
00328 r10 = eerot[1*3+0];
00329 r11 = eerot[1*3+1];
00330 r12 = eerot[1*3+2];
00331 r20 = eerot[2*3+0];
00332 r21 = eerot[2*3+1];
00333 r22 = eerot[2*3+2];
00334 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
00335
00336 new_r00=((-1.00000000000000)*(r02));
00337 new_r01=r01;
00338 new_r02=r00;
00339 new_px=((((-0.0800000000000000)*(r00)))+(px));
00340 new_r10=((-1.00000000000000)*(r12));
00341 new_r11=r11;
00342 new_r12=r10;
00343 new_py=((((-0.0800000000000000)*(r10)))+(py));
00344 new_r20=((-1.00000000000000)*(r22));
00345 new_r21=r21;
00346 new_r22=r20;
00347 new_pz=((-0.390950000000000)+(((-0.0800000000000000)*(r20)))+(pz));
00348 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;
00349 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
00350 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
00351 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
00352 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
00353 rxp0_0=((((-1.00000000000000)*(py)*(r20)))+(((pz)*(r10))));
00354 rxp0_1=((((px)*(r20)))+(((-1.00000000000000)*(pz)*(r00))));
00355 rxp0_2=((((-1.00000000000000)*(px)*(r10)))+(((py)*(r00))));
00356 rxp1_0=((((-1.00000000000000)*(py)*(r21)))+(((pz)*(r11))));
00357 rxp1_1=((((px)*(r21)))+(((-1.00000000000000)*(pz)*(r01))));
00358 rxp1_2=((((-1.00000000000000)*(px)*(r11)))+(((py)*(r01))));
00359 rxp2_0=((((-1.00000000000000)*(py)*(r22)))+(((pz)*(r12))));
00360 rxp2_1=((((px)*(r22)))+(((-1.00000000000000)*(pz)*(r02))));
00361 rxp2_2=((((-1.00000000000000)*(px)*(r12)))+(((py)*(r02))));
00362 {
00363 IKReal j0array[2], cj0array[2], sj0array[2];
00364 bool j0valid[2]={false};
00365 IKReal x51=((-1.00000000000000)*(py));
00366 IKReal x52=IKatan2(x51, px);
00367 j0array[0]=((-1.00000000000000)*(x52));
00368 sj0array[0]=IKsin(j0array[0]);
00369 cj0array[0]=IKcos(j0array[0]);
00370 j0array[1]=((3.14159265358979)+(((-1.00000000000000)*(x52))));
00371 sj0array[1]=IKsin(j0array[1]);
00372 cj0array[1]=IKcos(j0array[1]);
00373 if( j0array[0] > IKPI )
00374 {
00375 j0array[0]-=IK2PI;
00376 }
00377 else if( j0array[0] < -IKPI )
00378 { j0array[0]+=IK2PI;
00379 }
00380 j0valid[0] = true;
00381 if( j0array[1] > IKPI )
00382 {
00383 j0array[1]-=IK2PI;
00384 }
00385 else if( j0array[1] < -IKPI )
00386 { j0array[1]+=IK2PI;
00387 }
00388 j0valid[1] = true;
00389 if( j0valid[0] && j0valid[1] && IKabs(cj0array[0]-cj0array[1]) < 0.0001 && IKabs(sj0array[0]-sj0array[1]) < 0.0001 )
00390 {
00391 j0valid[1]=false;
00392 }
00393 for(int ij0 = 0; ij0 < 2; ++ij0)
00394 {
00395 if( !j0valid[ij0] )
00396 {
00397 continue;
00398 }
00399 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
00400
00401 {
00402 IKReal j2array[2], cj2array[2], sj2array[2];
00403 bool j2valid[2]={false};
00404 IKReal x53=((0.888629283483560)*(py)*(sj0));
00405 IKReal x54=((0.888629283483560)*(cj0)*(px));
00406 IKReal x55=((0.975985144601143)+(x54)+(x53));
00407 IKReal x56=((6.00425191542946)*(pp));
00408 IKReal x57=((x55)+(((-1.00000000000000)*(x56))));
00409 if( (x57) < -1-IKFAST_SINCOS_THRESH || (x57) > 1+IKFAST_SINCOS_THRESH )
00410 continue;
00411 IKReal x58=IKasin(x57);
00412 j2array[0]=((-2.84547750771483)+(((-1.00000000000000)*(x58))));
00413 sj2array[0]=IKsin(j2array[0]);
00414 cj2array[0]=IKcos(j2array[0]);
00415 j2array[1]=((0.296115145874965)+(x58));
00416 sj2array[1]=IKsin(j2array[1]);
00417 cj2array[1]=IKcos(j2array[1]);
00418 if( j2array[0] > IKPI )
00419 {
00420 j2array[0]-=IK2PI;
00421 }
00422 else if( j2array[0] < -IKPI )
00423 { j2array[0]+=IK2PI;
00424 }
00425 j2valid[0] = true;
00426 if( j2array[1] > IKPI )
00427 {
00428 j2array[1]-=IK2PI;
00429 }
00430 else if( j2array[1] < -IKPI )
00431 { j2array[1]+=IK2PI;
00432 }
00433 j2valid[1] = true;
00434 if( j2valid[0] && j2valid[1] && IKabs(cj2array[0]-cj2array[1]) < 0.0001 && IKabs(sj2array[0]-sj2array[1]) < 0.0001 )
00435 {
00436 j2valid[1]=false;
00437 }
00438 for(int ij2 = 0; ij2 < 2; ++ij2)
00439 {
00440 if( !j2valid[ij2] )
00441 {
00442 continue;
00443 }
00444 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
00445
00446 {
00447 IKReal dummyeval[1];
00448 IKReal gconst0;
00449 gconst0=IKsign(((0.0199800000000000)+(((-0.270000000000000)*(py)*(sj0)))+(((-0.0900000000000000)*(pz)*(sj2)))+(((-0.0900000000000000)*(cj2)*(py)*(sj0)))+(((-0.270000000000000)*(cj0)*(px)))+(((0.295000000000000)*(py)*(sj0)*(sj2)))+(((-0.0218300000000000)*(sj2)))+(((-0.295000000000000)*(cj2)*(pz)))+(((-0.0900000000000000)*(cj0)*(cj2)*(px)))+(((0.00666000000000000)*(cj2)))+(((0.295000000000000)*(cj0)*(px)*(sj2)))));
00450 dummyeval[0]=((3.00000000000000)+(((-13.5135135135135)*(pz)*(sj2)))+(cj2)+(((-3.27777777777778)*(sj2)))+(((-13.5135135135135)*(cj2)*(py)*(sj0)))+(((-40.5405405405405)*(py)*(sj0)))+(((-13.5135135135135)*(cj0)*(cj2)*(px)))+(((-44.2942942942943)*(cj2)*(pz)))+(((44.2942942942943)*(py)*(sj0)*(sj2)))+(((44.2942942942943)*(cj0)*(px)*(sj2)))+(((-40.5405405405405)*(cj0)*(px))));
00451 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00452 {
00453 continue;
00454
00455 } else
00456 {
00457 {
00458 IKReal j1array[1], cj1array[1], sj1array[1];
00459 bool j1valid[1]={false};
00460 IKReal x59=(sj2)*(sj2);
00461 IKReal x60=(cj2)*(cj2);
00462 j1array[0]=IKatan2(((-1.00000000000000)*(gconst0)*(((0.0729000000000000)+(((-1.00000000000000)*((pz)*(pz))))+(((-0.159300000000000)*(sj2)))+(((0.0486000000000000)*(cj2)))+(((-0.0531000000000000)*(cj2)*(sj2)))+(((0.00810000000000000)*(x60)))+(((0.0870250000000000)*(x59)))))), ((gconst0)*(((((-0.0243000000000000)*(sj2)))+(((0.0789250000000000)*(cj2)*(sj2)))+(((0.0740000000000000)*(pz)))+(((-1.00000000000000)*(cj0)*(px)*(pz)))+(((-0.0265500000000000)*(x60)))+(((-0.0796500000000000)*(cj2)))+(((-1.00000000000000)*(py)*(pz)*(sj0)))+(((0.0265500000000000)*(x59)))))));
00463 sj1array[0]=IKsin(j1array[0]);
00464 cj1array[0]=IKcos(j1array[0]);
00465 if( j1array[0] > IKPI )
00466 {
00467 j1array[0]-=IK2PI;
00468 }
00469 else if( j1array[0] < -IKPI )
00470 { j1array[0]+=IK2PI;
00471 }
00472 j1valid[0] = true;
00473 for(int ij1 = 0; ij1 < 1; ++ij1)
00474 {
00475 if( !j1valid[ij1] )
00476 {
00477 continue;
00478 }
00479 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
00480
00481 rotationfunction0(vsolutions);
00482 }
00483 }
00484
00485 }
00486
00487 }
00488 }
00489 }
00490 }
00491 }
00492 }
00493 return vsolutions.size()>0;
00494 }
00495 inline void rotationfunction0(std::vector<IKSolution>& vsolutions) {
00496 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
00497 IKReal x61=((cj2)*(sj1));
00498 IKReal x62=((cj1)*(sj2));
00499 IKReal x63=((x61)+(x62));
00500 IKReal x64=((-1.00000000000000)*(x63));
00501 IKReal x65=((sj1)*(sj2));
00502 IKReal x66=((cj1)*(cj2));
00503 IKReal x67=((x65)+(((-1.00000000000000)*(x66))));
00504 IKReal x68=((x66)+(((-1.00000000000000)*(x65))));
00505 new_r00=((((r20)*(x67)))+(((cj0)*(r00)*(x64)))+(((r10)*(sj0)*(x64))));
00506 new_r01=((((r21)*(x67)))+(((cj0)*(r01)*(x64)))+(((r11)*(sj0)*(x64))));
00507 new_r02=((((r12)*(sj0)*(x64)))+(((r22)*(x67)))+(((cj0)*(r02)*(x64))));
00508 new_r10=((((-1.00000000000000)*(r00)*(sj0)))+(((cj0)*(r10))));
00509 new_r11=((((-1.00000000000000)*(r01)*(sj0)))+(((cj0)*(r11))));
00510 new_r12=((((cj0)*(r12)))+(((-1.00000000000000)*(r02)*(sj0))));
00511 new_r20=((((r20)*(x64)))+(((cj0)*(r00)*(x68)))+(((r10)*(sj0)*(x68))));
00512 new_r21=((((r21)*(x64)))+(((cj0)*(r01)*(x68)))+(((r11)*(sj0)*(x68))));
00513 new_r22=((((r12)*(sj0)*(x68)))+(((r22)*(x64)))+(((cj0)*(r02)*(x68))));
00514 {
00515 IKReal j4array[2], cj4array[2], sj4array[2];
00516 bool j4valid[2]={false};
00517 cj4array[0]=new_r22;
00518 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
00519 {
00520 j4valid[0] = j4valid[1] = true;
00521 j4array[0] = IKacos(cj4array[0]);
00522 sj4array[0] = IKsin(j4array[0]);
00523 cj4array[1] = cj4array[0];
00524 j4array[1] = -j4array[0];
00525 sj4array[1] = -sj4array[0];
00526 }
00527 else if( isnan(cj4array[0]) )
00528 {
00529
00530 j4valid[0] = true;
00531 cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
00532 }
00533 if( j4valid[0] && j4valid[1] && IKabs(cj4array[0]-cj4array[1]) < 0.0001 && IKabs(sj4array[0]-sj4array[1]) < 0.0001 )
00534 {
00535 j4valid[1]=false;
00536 }
00537 for(int ij4 = 0; ij4 < 2; ++ij4)
00538 {
00539 if( !j4valid[ij4] )
00540 {
00541 continue;
00542 }
00543 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
00544
00545 {
00546 IKReal dummyeval[1];
00547 IKReal gconst2;
00548 gconst2=IKsign(sj4);
00549 dummyeval[0]=sj4;
00550 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00551 {
00552 {
00553 IKReal dummyeval[1];
00554 IKReal gconst1;
00555 gconst1=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
00556 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
00557 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00558 {
00559 {
00560 IKReal evalcond[7];
00561 IKReal x69=((-1.00000000000000)+(new_r22));
00562 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j4)), 6.28318530717959)));
00563 evalcond[1]=x69;
00564 evalcond[2]=new_r20;
00565 evalcond[3]=new_r21;
00566 evalcond[4]=new_r20;
00567 evalcond[5]=new_r21;
00568 evalcond[6]=x69;
00569 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 )
00570 {
00571 {
00572 IKReal j3array[2], cj3array[2], sj3array[2];
00573 bool j3valid[2]={false};
00574 IKReal x70=IKatan2(new_r02, new_r12);
00575 j3array[0]=((-1.00000000000000)*(x70));
00576 sj3array[0]=IKsin(j3array[0]);
00577 cj3array[0]=IKcos(j3array[0]);
00578 j3array[1]=((3.14159265358979)+(((-1.00000000000000)*(x70))));
00579 sj3array[1]=IKsin(j3array[1]);
00580 cj3array[1]=IKcos(j3array[1]);
00581 if( j3array[0] > IKPI )
00582 {
00583 j3array[0]-=IK2PI;
00584 }
00585 else if( j3array[0] < -IKPI )
00586 { j3array[0]+=IK2PI;
00587 }
00588 j3valid[0] = true;
00589 if( j3array[1] > IKPI )
00590 {
00591 j3array[1]-=IK2PI;
00592 }
00593 else if( j3array[1] < -IKPI )
00594 { j3array[1]+=IK2PI;
00595 }
00596 j3valid[1] = true;
00597 if( j3valid[0] && j3valid[1] && IKabs(cj3array[0]-cj3array[1]) < 0.0001 && IKabs(sj3array[0]-sj3array[1]) < 0.0001 )
00598 {
00599 j3valid[1]=false;
00600 }
00601 for(int ij3 = 0; ij3 < 2; ++ij3)
00602 {
00603 if( !j3valid[ij3] )
00604 {
00605 continue;
00606 }
00607 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00608
00609 {
00610 IKReal j5array[1], cj5array[1], sj5array[1];
00611 bool j5valid[1]={false};
00612 j5array[0]=IKatan2(((((-1.00000000000000)*(cj3)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
00613 sj5array[0]=IKsin(j5array[0]);
00614 cj5array[0]=IKcos(j5array[0]);
00615 if( j5array[0] > IKPI )
00616 {
00617 j5array[0]-=IK2PI;
00618 }
00619 else if( j5array[0] < -IKPI )
00620 { j5array[0]+=IK2PI;
00621 }
00622 j5valid[0] = true;
00623 for(int ij5 = 0; ij5 < 1; ++ij5)
00624 {
00625 if( !j5valid[ij5] )
00626 {
00627 continue;
00628 }
00629 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00630
00631 {
00632 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
00633 solution.basesol.resize(6);
00634 solution.basesol[0].foffset = j0;
00635 solution.basesol[1].foffset = j1;
00636 solution.basesol[2].foffset = j2;
00637 solution.basesol[3].foffset = j3;
00638 solution.basesol[4].foffset = j4;
00639 solution.basesol[5].foffset = j5;
00640 solution.vfree.resize(0);
00641 }
00642 }
00643 }
00644 }
00645 }
00646
00647 } else
00648 {
00649 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j4)), 6.28318530717959)));
00650 evalcond[1]=((1.00000000000000)+(new_r22));
00651 evalcond[2]=new_r20;
00652 evalcond[3]=new_r21;
00653 evalcond[4]=((-1.00000000000000)*(new_r20));
00654 evalcond[5]=((-1.00000000000000)*(new_r21));
00655 evalcond[6]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
00656 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 )
00657 {
00658 {
00659 IKReal j3array[2], cj3array[2], sj3array[2];
00660 bool j3valid[2]={false};
00661 IKReal x71=IKatan2(new_r02, new_r12);
00662 j3array[0]=((-1.00000000000000)*(x71));
00663 sj3array[0]=IKsin(j3array[0]);
00664 cj3array[0]=IKcos(j3array[0]);
00665 j3array[1]=((3.14159265358979)+(((-1.00000000000000)*(x71))));
00666 sj3array[1]=IKsin(j3array[1]);
00667 cj3array[1]=IKcos(j3array[1]);
00668 if( j3array[0] > IKPI )
00669 {
00670 j3array[0]-=IK2PI;
00671 }
00672 else if( j3array[0] < -IKPI )
00673 { j3array[0]+=IK2PI;
00674 }
00675 j3valid[0] = true;
00676 if( j3array[1] > IKPI )
00677 {
00678 j3array[1]-=IK2PI;
00679 }
00680 else if( j3array[1] < -IKPI )
00681 { j3array[1]+=IK2PI;
00682 }
00683 j3valid[1] = true;
00684 if( j3valid[0] && j3valid[1] && IKabs(cj3array[0]-cj3array[1]) < 0.0001 && IKabs(sj3array[0]-sj3array[1]) < 0.0001 )
00685 {
00686 j3valid[1]=false;
00687 }
00688 for(int ij3 = 0; ij3 < 2; ++ij3)
00689 {
00690 if( !j3valid[ij3] )
00691 {
00692 continue;
00693 }
00694 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00695
00696 {
00697 IKReal j5array[1], cj5array[1], sj5array[1];
00698 bool j5valid[1]={false};
00699 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((-1.00000000000000)*(cj3)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj3)))));
00700 sj5array[0]=IKsin(j5array[0]);
00701 cj5array[0]=IKcos(j5array[0]);
00702 if( j5array[0] > IKPI )
00703 {
00704 j5array[0]-=IK2PI;
00705 }
00706 else if( j5array[0] < -IKPI )
00707 { j5array[0]+=IK2PI;
00708 }
00709 j5valid[0] = true;
00710 for(int ij5 = 0; ij5 < 1; ++ij5)
00711 {
00712 if( !j5valid[ij5] )
00713 {
00714 continue;
00715 }
00716 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00717
00718 {
00719 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
00720 solution.basesol.resize(6);
00721 solution.basesol[0].foffset = j0;
00722 solution.basesol[1].foffset = j1;
00723 solution.basesol[2].foffset = j2;
00724 solution.basesol[3].foffset = j3;
00725 solution.basesol[4].foffset = j4;
00726 solution.basesol[5].foffset = j5;
00727 solution.vfree.resize(0);
00728 }
00729 }
00730 }
00731 }
00732 }
00733
00734 } else
00735 {
00736 if( 1 )
00737 {
00738 continue;
00739
00740 } else
00741 {
00742 }
00743 }
00744 }
00745 }
00746
00747 } else
00748 {
00749 {
00750 IKReal j3array[1], cj3array[1], sj3array[1];
00751 bool j3valid[1]={false};
00752 j3array[0]=IKatan2(((gconst1)*(new_r12)*(sj4)), ((gconst1)*(new_r02)*(sj4)));
00753 sj3array[0]=IKsin(j3array[0]);
00754 cj3array[0]=IKcos(j3array[0]);
00755 if( j3array[0] > IKPI )
00756 {
00757 j3array[0]-=IK2PI;
00758 }
00759 else if( j3array[0] < -IKPI )
00760 { j3array[0]+=IK2PI;
00761 }
00762 j3valid[0] = true;
00763 for(int ij3 = 0; ij3 < 1; ++ij3)
00764 {
00765 if( !j3valid[ij3] )
00766 {
00767 continue;
00768 }
00769 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
00770
00771 {
00772 IKReal dummyeval[1];
00773 IKReal gconst3;
00774 gconst3=IKsign(sj4);
00775 dummyeval[0]=sj4;
00776 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00777 {
00778 {
00779 IKReal evalcond[11];
00780 IKReal x72=((cj3)*(new_r12));
00781 IKReal x73=((new_r02)*(sj3));
00782 IKReal x74=((((-1.00000000000000)*(x73)))+(x72));
00783 IKReal x75=((-1.00000000000000)+(new_r22));
00784 IKReal x76=((new_r12)*(sj3));
00785 IKReal x77=((cj3)*(new_r02));
00786 IKReal x78=((x77)+(x76));
00787 evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(j4)), 6.28318530717959)));
00788 evalcond[1]=x75;
00789 evalcond[2]=new_r20;
00790 evalcond[3]=new_r21;
00791 evalcond[4]=x74;
00792 evalcond[5]=x74;
00793 evalcond[6]=x78;
00794 evalcond[7]=new_r20;
00795 evalcond[8]=new_r21;
00796 evalcond[9]=x75;
00797 evalcond[10]=x78;
00798 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 )
00799 {
00800 {
00801 IKReal j5array[1], cj5array[1], sj5array[1];
00802 bool j5valid[1]={false};
00803 j5array[0]=IKatan2(((((-1.00000000000000)*(cj3)*(new_r01)))+(((-1.00000000000000)*(new_r11)*(sj3)))), ((((new_r10)*(sj3)))+(((cj3)*(new_r00)))));
00804 sj5array[0]=IKsin(j5array[0]);
00805 cj5array[0]=IKcos(j5array[0]);
00806 if( j5array[0] > IKPI )
00807 {
00808 j5array[0]-=IK2PI;
00809 }
00810 else if( j5array[0] < -IKPI )
00811 { j5array[0]+=IK2PI;
00812 }
00813 j5valid[0] = true;
00814 for(int ij5 = 0; ij5 < 1; ++ij5)
00815 {
00816 if( !j5valid[ij5] )
00817 {
00818 continue;
00819 }
00820 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00821
00822 {
00823 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
00824 solution.basesol.resize(6);
00825 solution.basesol[0].foffset = j0;
00826 solution.basesol[1].foffset = j1;
00827 solution.basesol[2].foffset = j2;
00828 solution.basesol[3].foffset = j3;
00829 solution.basesol[4].foffset = j4;
00830 solution.basesol[5].foffset = j5;
00831 solution.vfree.resize(0);
00832 }
00833 }
00834 }
00835
00836 } else
00837 {
00838 IKReal x79=((cj3)*(new_r12));
00839 IKReal x80=((new_r02)*(sj3));
00840 IKReal x81=((x79)+(((-1.00000000000000)*(x80))));
00841 IKReal x82=((new_r12)*(sj3));
00842 IKReal x83=((cj3)*(new_r02));
00843 IKReal x84=((x82)+(x83));
00844 evalcond[0]=((-3.14159265358979)+(IKfmod(((1.11022302462516e-16)+(j4)), 6.28318530717959)));
00845 evalcond[1]=((1.00000000000000)+(new_r22));
00846 evalcond[2]=new_r20;
00847 evalcond[3]=new_r21;
00848 evalcond[4]=x81;
00849 evalcond[5]=x81;
00850 evalcond[6]=x84;
00851 evalcond[7]=((-1.00000000000000)*(new_r20));
00852 evalcond[8]=((-1.00000000000000)*(new_r21));
00853 evalcond[9]=((-1.00000000000000)+(((-1.00000000000000)*(new_r22))));
00854 evalcond[10]=((-1.00000000000000)*(x84));
00855 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 )
00856 {
00857 {
00858 IKReal j5array[1], cj5array[1], sj5array[1];
00859 bool j5valid[1]={false};
00860 j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((-1.00000000000000)*(cj3)*(new_r00)))+(((-1.00000000000000)*(new_r10)*(sj3)))));
00861 sj5array[0]=IKsin(j5array[0]);
00862 cj5array[0]=IKcos(j5array[0]);
00863 if( j5array[0] > IKPI )
00864 {
00865 j5array[0]-=IK2PI;
00866 }
00867 else if( j5array[0] < -IKPI )
00868 { j5array[0]+=IK2PI;
00869 }
00870 j5valid[0] = true;
00871 for(int ij5 = 0; ij5 < 1; ++ij5)
00872 {
00873 if( !j5valid[ij5] )
00874 {
00875 continue;
00876 }
00877 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00878
00879 {
00880 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
00881 solution.basesol.resize(6);
00882 solution.basesol[0].foffset = j0;
00883 solution.basesol[1].foffset = j1;
00884 solution.basesol[2].foffset = j2;
00885 solution.basesol[3].foffset = j3;
00886 solution.basesol[4].foffset = j4;
00887 solution.basesol[5].foffset = j5;
00888 solution.vfree.resize(0);
00889 }
00890 }
00891 }
00892
00893 } else
00894 {
00895 if( 1 )
00896 {
00897 continue;
00898
00899 } else
00900 {
00901 }
00902 }
00903 }
00904 }
00905
00906 } else
00907 {
00908 {
00909 IKReal j5array[1], cj5array[1], sj5array[1];
00910 bool j5valid[1]={false};
00911 j5array[0]=IKatan2(((gconst3)*(new_r21)), ((-1.00000000000000)*(gconst3)*(new_r20)));
00912 sj5array[0]=IKsin(j5array[0]);
00913 cj5array[0]=IKcos(j5array[0]);
00914 if( j5array[0] > IKPI )
00915 {
00916 j5array[0]-=IK2PI;
00917 }
00918 else if( j5array[0] < -IKPI )
00919 { j5array[0]+=IK2PI;
00920 }
00921 j5valid[0] = true;
00922 for(int ij5 = 0; ij5 < 1; ++ij5)
00923 {
00924 if( !j5valid[ij5] )
00925 {
00926 continue;
00927 }
00928 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00929
00930 {
00931 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
00932 solution.basesol.resize(6);
00933 solution.basesol[0].foffset = j0;
00934 solution.basesol[1].foffset = j1;
00935 solution.basesol[2].foffset = j2;
00936 solution.basesol[3].foffset = j3;
00937 solution.basesol[4].foffset = j4;
00938 solution.basesol[5].foffset = j5;
00939 solution.vfree.resize(0);
00940 }
00941 }
00942 }
00943
00944 }
00945
00946 }
00947 }
00948 }
00949
00950 }
00951
00952 }
00953
00954 } else
00955 {
00956 {
00957 IKReal j5array[1], cj5array[1], sj5array[1];
00958 bool j5valid[1]={false};
00959 j5array[0]=IKatan2(((gconst2)*(new_r21)), ((-1.00000000000000)*(gconst2)*(new_r20)));
00960 sj5array[0]=IKsin(j5array[0]);
00961 cj5array[0]=IKcos(j5array[0]);
00962 if( j5array[0] > IKPI )
00963 {
00964 j5array[0]-=IK2PI;
00965 }
00966 else if( j5array[0] < -IKPI )
00967 { j5array[0]+=IK2PI;
00968 }
00969 j5valid[0] = true;
00970 for(int ij5 = 0; ij5 < 1; ++ij5)
00971 {
00972 if( !j5valid[ij5] )
00973 {
00974 continue;
00975 }
00976 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
00977
00978 {
00979 IKReal dummyeval[1];
00980 IKReal gconst4;
00981 gconst4=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02))));
00982 dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02)));
00983 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
00984 {
00985 continue;
00986
00987 } else
00988 {
00989 {
00990 IKReal j3array[1], cj3array[1], sj3array[1];
00991 bool j3valid[1]={false};
00992 j3array[0]=IKatan2(((gconst4)*(new_r12)*(sj4)), ((gconst4)*(new_r02)*(sj4)));
00993 sj3array[0]=IKsin(j3array[0]);
00994 cj3array[0]=IKcos(j3array[0]);
00995 if( j3array[0] > IKPI )
00996 {
00997 j3array[0]-=IK2PI;
00998 }
00999 else if( j3array[0] < -IKPI )
01000 { j3array[0]+=IK2PI;
01001 }
01002 j3valid[0] = true;
01003 for(int ij3 = 0; ij3 < 1; ++ij3)
01004 {
01005 if( !j3valid[ij3] )
01006 {
01007 continue;
01008 }
01009 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
01010
01011 {
01012 vsolutions.push_back(IKSolution()); IKSolution& solution = vsolutions.back();
01013 solution.basesol.resize(6);
01014 solution.basesol[0].foffset = j0;
01015 solution.basesol[1].foffset = j1;
01016 solution.basesol[2].foffset = j2;
01017 solution.basesol[3].foffset = j3;
01018 solution.basesol[4].foffset = j4;
01019 solution.basesol[5].foffset = j5;
01020 solution.vfree.resize(0);
01021 }
01022 }
01023 }
01024
01025 }
01026
01027 }
01028 }
01029 }
01030
01031 }
01032
01033 }
01034 }
01035 }
01036 }
01037 }};
01038
01039
01042 IKFAST_API bool ik(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<IKSolution>& vsolutions) {
01043 IKSolver solver;
01044 return solver.ik(eetrans,eerot,pfree,vsolutions);
01045 }
01046
01047 IKFAST_API const char* getKinematicsHash() { return "204673ec97731ed369d9c210d57f3890"; }
01048
01049 #ifdef IKFAST_NAMESPACE
01050 }
01051 #endif
01052
01053 #ifndef IKFAST_NO_MAIN
01054 #include <stdio.h>
01055 #include <stdlib.h>
01056 #ifdef IKFAST_NAMESPACE
01057 using namespace IKFAST_NAMESPACE;
01058 #endif
01059 int main(int argc, char** argv)
01060 {
01061 if( argc != 12+getNumFreeParameters()+1 ) {
01062 printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
01063 "Returns the ik solutions given the transformation of the end effector specified by\n"
01064 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
01065 "There are %d free parameters that have to be specified.\n\n",getNumFreeParameters());
01066 return 1;
01067 }
01068
01069 std::vector<IKSolution> vsolutions;
01070 std::vector<IKReal> vfree(getNumFreeParameters());
01071 IKReal eerot[9],eetrans[3];
01072 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
01073 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
01074 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
01075 for(std::size_t i = 0; i < vfree.size(); ++i)
01076 vfree[i] = atof(argv[13+i]);
01077 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
01078
01079 if( !bSuccess ) {
01080 fprintf(stderr,"Failed to get ik solution\n");
01081 return -1;
01082 }
01083
01084 printf("Found %d ik solutions:\n", (int)vsolutions.size());
01085 std::vector<IKReal> sol(getNumJoints());
01086 for(std::size_t i = 0; i < vsolutions.size(); ++i) {
01087 printf("sol%d (free=%d): ", (int)i, (int)vsolutions[i].GetFree().size());
01088 std::vector<IKReal> vsolfree(vsolutions[i].GetFree().size());
01089 vsolutions[i].GetSolution(&sol[0],vsolfree.size()>0?&vsolfree[0]:NULL);
01090 for( std::size_t j = 0; j < sol.size(); ++j)
01091 printf("%.15f, ", sol[j]);
01092 printf("\n");
01093 }
01094 return 0;
01095 }
01096
01097 #endif