1 #define IKFAST_HAS_LIBRARY 26 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] 41 #ifndef __PRETTY_FUNCTION__ 42 #define __PRETTY_FUNCTION__ __FUNCDNAME__ 46 #ifndef __PRETTY_FUNCTION__ 47 #define __PRETTY_FUNCTION__ __func__ 50 #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()); } } 55 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x 57 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16))) 60 #define IK2PI ((IkReal)6.28318530717959) 61 #define IKPI ((IkReal)3.14159265358979) 62 #define IKPI_2 ((IkReal)1.57079632679490) 78 void dgetrf_ (
const int* m,
const int* n,
double* a,
const int* lda,
int* ipiv,
int* info);
79 void zgetrf_ (
const int* m,
const int* n, std::complex<double>* a,
const int* lda,
int* ipiv,
int* info);
80 void dgetri_(
const int* n,
const double* a,
const int* lda,
int* ipiv,
double* work,
const int* lwork,
int* info);
81 void dgesv_ (
const int* n,
const int* nrhs,
double* a,
const int* lda,
int* ipiv,
double* b,
const int* ldb,
int* info);
82 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);
83 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);
88 #ifdef IKFAST_NAMESPACE 89 namespace IKFAST_NAMESPACE {
92 inline float IKabs(
float f) {
return fabsf(f); }
93 inline double IKabs(
double f) {
return fabs(f); }
95 inline float IKsqr(
float f) {
return f*f; }
96 inline double IKsqr(
double f) {
return f*f; }
98 inline float IKlog(
float f) {
return logf(f); }
99 inline double IKlog(
double f) {
return log(f); }
102 #ifndef IKFAST_SINCOS_THRESH 103 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7) 107 #ifndef IKFAST_ATAN2_MAGTHRESH 108 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7) 112 #ifndef IKFAST_SOLUTION_THRESH 113 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) 117 #ifndef IKFAST_EVALCOND_THRESH 118 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001) 125 if( f <= -1 )
return float(-
IKPI_2);
126 else if( f >= 1 )
return float(
IKPI_2);
132 if( f <= -1 )
return -
IKPI_2;
133 else if( f >= 1 )
return IKPI_2;
158 if( f <= -1 )
return float(
IKPI);
159 else if( f >= 1 )
return float(0);
165 if( f <= -1 )
return IKPI;
166 else if( f >= 1 )
return 0;
169 inline float IKsin(
float f) {
return sinf(f); }
171 inline float IKcos(
float f) {
return cosf(f); }
173 inline float IKtan(
float f) {
return tanf(f); }
175 inline float IKsqrt(
float f) {
if( f <= 0.0f )
return 0.0f;
return sqrtf(f); }
176 inline double IKsqrt(
double f) {
if( f <= 0.0 )
return 0.0;
return sqrt(f); }
178 return atan2f(fy,fx);
185 else if( isnan(fx) ) {
188 return atan2f(fy,fx);
198 else if( isnan(fx) ) {
204 template <
typename T>
211 template <
typename T>
217 if( !isnan(fy) && !isnan(fx) ) {
246 template <
typename T>
265 ret.
value = (T)1.0e30;
269 ret.
value = T(1.0)/f;
274 int num = n > 0 ? n : -n;
278 else if( num == 3 ) {
300 IKFAST_API
void ComputeFk(
const IkReal* j, IkReal* eetrans, IkReal* eerot) {
301 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;
336 x34=((((-1.0)*x33))+x23);
337 x35=(x0*(((((-1.0)*x33))+x23)));
338 x36=((((-1.0)*x12*x25))+((x23*x5)));
339 x37=(((x19*x26))+((x12*x24)));
340 x38=((((1.0)*x1*x25))+((x12*x28)));
344 x42=(((x10*x34*x7))+((x11*((x30+x26)))));
345 x43=(((x18*(((((-1.0)*x31))+((x20*x37))))))+(((-1.0)*x13*x35)));
346 x44=(((x18*((((x20*x38))+((x19*x6))))))+(((-1.0)*x13*x36)));
347 eerot[0]=(((x8*(((((-1.0)*x22*x37))+(((-1.0)*x20*x5))))))+((x43*x9)));
348 eerot[1]=(((x9*((((x5*x7))+x40))))+((x43*x8)));
349 eerot[2]=(((x10*x35))+((x11*(((((-1.0)*x31))+x41)))));
350 IkReal x45=((1.0)*x27);
351 eetrans[0]=((-3.153)+(((0.055)*x0*x26))+(((-0.35)*x0))+(((-1.0)*x0*x16))+((x17*x23))+((x10*((((x21*x23))+(((-1.0)*x21*x45))))))+(((-1.0)*x17*x45))+((x11*(((((-1.0)*x14*x6))+(((0.23)*x41))))))+j[0]+((x15*x24)));
352 eerot[3]=(((x8*(((((-1.0)*x22*x38))+x32))))+((x44*x9)));
353 eerot[4]=(((x9*(((((-1.0)*x32))+((x38*x6))))))+((x44*x8)));
354 eerot[5]=(((x11*((((x0*x6))+x39))))+((x10*x36)));
355 eetrans[1]=((-0.212)+(((0.055)*x1*x25))+((x10*((((x14*x23))+(((-1.0)*x14*x27))))))+(((-0.35)*x5))+(((-1.0)*x16*x5))+(((-1.1)*x25*x3))+((x11*((((x21*x6))+(((0.23)*x39))))))+((x15*x28))+(((1.1)*x23*x5)));
356 eerot[6]=(((x6*x8*(((((-1.0)*x23))+x33))))+((x42*x9)));
357 eerot[7]=(((x42*x8))+((x34*x6*x9)));
358 eerot[8]=(((x29*x34))+((x10*(((((-1.0)*x12*x2))+(((-1.0)*x26)))))));
359 eetrans[2]=((1.69)+(((-1.1)*x26))+((x29*(((((0.23)*x23))+(((-0.23)*x27))))))+(((-1.25)*x1))+(((0.055)*x23))+((x10*(((((-0.23)*x30))+(((-0.23)*x26))))))+(((-1.0)*x15*x4))+(((-1.1)*x30)));
372 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,j6,cj6,sj6,htj6,j6mul,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;
373 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5,_ij6[2], _nj6,_ij2[2], _nj2;
375 IkReal j100, cj100, sj100;
376 unsigned char _ij100[2], _nj100;
378 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; j6=numeric_limits<IkReal>::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; _ij2[0] = -1; _ij2[1] = -1; _nj2 = 0;
379 for(
int dummyiter = 0; dummyiter < 1; ++dummyiter) {
381 j2=pfree[0]; cj2=
cos(pfree[0]); sj2=
sin(pfree[0]), htj2=
tan(pfree[0]*0.5);
391 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
394 new_r01=((-1.0)*r21);
396 new_px=((1.69)+(((0.23)*r22))+(((-1.0)*pz)));
397 new_r10=((-1.0)*r10);
399 new_r12=((-1.0)*r12);
400 new_py=((0.212)+(((-0.23)*r12))+py);
401 new_r20=((-1.0)*r00);
403 new_r22=((-1.0)*r02);
404 new_pz=((3.153)+(((-0.23)*r02))+px);
405 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;
406 IkReal x46=((1.0)*px);
407 IkReal x47=((1.0)*pz);
408 IkReal x48=((1.0)*py);
409 pp=((px*px)+(py*py)+(pz*pz));
410 npx=(((px*r00))+((py*r10))+((pz*r20)));
411 npy=(((px*r01))+((py*r11))+((pz*r21)));
412 npz=(((px*r02))+((py*r12))+((pz*r22)));
413 rxp0_0=((((-1.0)*r20*x48))+((pz*r10)));
414 rxp0_1=(((px*r20))+(((-1.0)*r00*x47)));
415 rxp0_2=((((-1.0)*r10*x46))+((py*r00)));
416 rxp1_0=((((-1.0)*r21*x48))+((pz*r11)));
417 rxp1_1=(((px*r21))+(((-1.0)*r01*x47)));
418 rxp1_2=((((-1.0)*r11*x46))+((py*r01)));
419 rxp2_0=((((-1.0)*r22*x48))+((pz*r12)));
420 rxp2_1=((((-1.0)*r02*x47))+((px*r22)));
421 rxp2_2=((((-1.0)*r12*x46))+((py*r02)));
424 j3eval[0]=((sj2*sj2)+(cj2*cj2));
425 if(
IKabs(j3eval[0]) < 0.0000010000000000 )
432 IkReal j3array[2], cj3array[2], sj3array[2];
433 bool j3valid[2]={
false};
435 IkReal x49=((((1.1)*cj2))+(((-0.055)*sj2)));
436 IkReal x50=((((1.1)*sj2))+(((0.055)*cj2)));
441 IkReal x51=((1.0)*(x53.
value));
442 if((((x50*x50)+(x49*x49))) < -0.00001)
450 IkReal x52=
IKasin(((x54.
value)*(((((-1.0)*px))+(((1.25)*sj2))))));
451 j3array[0]=((((-1.0)*x51))+(((-1.0)*x52)));
452 sj3array[0]=
IKsin(j3array[0]);
453 cj3array[0]=
IKcos(j3array[0]);
454 j3array[1]=((3.14159265358979)+x52+(((-1.0)*x51)));
455 sj3array[1]=
IKsin(j3array[1]);
456 cj3array[1]=
IKcos(j3array[1]);
457 if( j3array[0] >
IKPI )
461 else if( j3array[0] < -
IKPI )
465 if( j3array[1] >
IKPI )
469 else if( j3array[1] < -
IKPI )
473 for(
int ij3 = 0; ij3 < 2; ++ij3)
479 _ij3[0] = ij3; _ij3[1] = -1;
480 for(
int iij3 = ij3+1; iij3 < 2; ++iij3)
484 j3valid[iij3]=
false; _ij3[1] = iij3;
break;
487 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
490 IkReal j0array[2], cj0array[2], sj0array[2];
491 bool j0valid[2]={
false};
493 if((((2.898025)+(((0.875)*cj2))+(((-0.77)*sj2*sj3))+(((-0.1375)*sj3))+(((-1.0)*pp))+(pz*pz)+(((-0.0385)*cj3*sj2))+(((-0.0385)*cj2*sj3))+(((2.75)*cj3))+(((0.77)*cj2*cj3)))) < -0.00001)
495 IkReal x55=
IKsqrt(((2.898025)+(((0.875)*cj2))+(((-0.77)*sj2*sj3))+(((-0.1375)*sj3))+(((-1.0)*pp))+(pz*pz)+(((-0.0385)*cj3*sj2))+(((-0.0385)*cj2*sj3))+(((2.75)*cj3))+(((0.77)*cj2*cj3))));
497 sj0array[0]=
IKsin(j0array[0]);
498 cj0array[0]=
IKcos(j0array[0]);
499 j0array[1]=(pz+(((-1.0)*x55)));
500 sj0array[1]=
IKsin(j0array[1]);
501 cj0array[1]=
IKcos(j0array[1]);
504 for(
int ij0 = 0; ij0 < 2; ++ij0)
510 _ij0[0] = ij0; _ij0[1] = -1;
511 for(
int iij0 = ij0+1; iij0 < 2; ++iij0)
515 j0valid[iij0]=
false; _ij0[1] = iij0;
break;
518 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
522 IkReal x56=(cj2*cj3);
523 IkReal x57=(cj3*sj2);
524 IkReal x58=(sj2*sj3);
525 IkReal x59=(cj2*sj3);
526 j1eval[0]=((6.36363636363636)+(((20.0)*x56))+(((22.7272727272727)*cj2))+(((-20.0)*x58))+(((-1.0)*x57))+(((-1.0)*x59)));
527 j1eval[1]=
IKsign(((0.35)+(((-0.055)*x57))+(((-0.055)*x59))+(((-1.1)*x58))+(((1.1)*x56))+(((1.25)*cj2))));
528 j1eval[2]=((
IKabs(py))+(
IKabs(((((-1.0)*pz))+j0))));
529 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
533 IkReal x60=((1.0)*sj2);
534 IkReal x61=((((-1.0)*x60*(j0*j0)))+((sj2*(px*px)))+(((-1.0)*pp*x60))+(((2.0)*j0*pz*sj2)));
537 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
541 IkReal x62=((((-2.0)*cj2*j0*pz))+(((-1.0)*cj2*(px*px)))+((cj2*(j0*j0)))+((cj2*pp)));
544 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
548 bool bgotonextstatement =
true;
551 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j2)))), 6.28318530717959)));
552 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
554 bgotonextstatement=
false;
560 j1eval[0]=((6.36363636363636)+(((-20.0)*sj3))+(((-1.0)*cj3)));
561 j1eval[1]=
IKsign(((0.35)+(((-0.055)*cj3))+(((-1.1)*sj3))));
562 j1eval[2]=((
IKabs(py))+(
IKabs(((((-1.0)*pz))+j0))));
563 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
570 IkReal x63=((1.1)*sj3);
571 IkReal x64=((0.055)*cj3);
572 IkReal x65=((px*px)+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((2.0)*j0*pz)));
575 j1eval[2]=((
IKabs(((((-0.35)*j0))+((j0*x63))+((j0*x64))+(((0.35)*pz))+(((-1.0)*pz*x64))+(((-1.0)*pz*x63)))))+(((0.005)*(
IKabs(((((-220.0)*py*sj3))+(((70.0)*py))+(((-11.0)*cj3*py))))))));
576 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
583 IkReal x66=((20.0)*sj3);
584 IkReal x67=((1.0)*cj3);
585 j1eval[0]=((6.36363636363636)+(((-1.0)*x67))+(((-1.0)*x66)));
586 j1eval[1]=((((6.36363636363636)*j0))+(((-1.0)*j0*x67))+(((-1.0)*j0*x66))+((cj3*pz))+((pz*x66))+(((-6.36363636363636)*pz)));
587 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
591 bool bgotonextstatement =
true;
594 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-2.76824195858973)+j3)))), 6.28318530717959)));
595 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
597 bgotonextstatement=
false;
599 IkReal j1array[1], cj1array[1], sj1array[1];
600 bool j1valid[1]={
false};
604 j1array[0]=
IKatan2(((((-1135096109.82221)*pz))+(((1135096109.82221)*j0))), ((-1135096109.82221)*py));
605 sj1array[0]=
IKsin(j1array[0]);
606 cj1array[0]=
IKcos(j1array[0]);
607 if( j1array[0] >
IKPI )
611 else if( j1array[0] < -
IKPI )
615 for(
int ij1 = 0; ij1 < 1; ++ij1)
621 _ij1[0] = ij1; _ij1[1] = -1;
622 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
626 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
629 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
632 IkReal x68=
IKcos(j1);
633 IkReal x69=
IKsin(j1);
634 IkReal x70=((1.0)*pz);
635 IkReal x71=((0.7)*x69);
637 evalcond[0]=(py+(((8.80982668645239e-10)*x68)));
638 evalcond[1]=(pz+(((8.80982668645239e-10)*x69))+(((-1.0)*j0)));
639 evalcond[2]=(((j0*x68))+(((-1.0)*x68*x70))+((py*x69)));
640 evalcond[3]=((-8.80982668645239e-10)+((j0*x69))+(((-1.0)*x72))+(((-1.0)*x69*x70)));
641 evalcond[4]=((-6.16687868051667e-10)+((j0*x71))+(((-0.7)*x72))+(((-1.0)*pz*x71)));
648 rotationfunction0(solutions);
654 if( bgotonextstatement )
656 bool bgotonextstatement =
true;
659 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-6.55661921073577)+j3)))), 6.28318530717959)));
660 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
662 bgotonextstatement=
false;
664 IkReal j1array[1], cj1array[1], sj1array[1];
665 bool j1valid[1]={
false};
669 j1array[0]=
IKatan2(((((-241222649.141269)*pz))+(((241222649.141269)*j0))), ((-241222649.141269)*py));
670 sj1array[0]=
IKsin(j1array[0]);
671 cj1array[0]=
IKcos(j1array[0]);
672 if( j1array[0] >
IKPI )
676 else if( j1array[0] < -
IKPI )
680 for(
int ij1 = 0; ij1 < 1; ++ij1)
686 _ij1[0] = ij1; _ij1[1] = -1;
687 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
691 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
694 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
697 IkReal x611=
IKcos(j1);
698 IkReal x612=
IKsin(j1);
699 IkReal x613=((1.0)*pz);
700 IkReal x614=((0.7)*x612);
701 IkReal x615=(py*x611);
702 evalcond[0]=(py+(((4.14554770690028e-9)*x611)));
703 evalcond[1]=(pz+(((-1.0)*j0))+(((4.14554770690028e-9)*x612)));
704 evalcond[2]=((((-1.0)*x611*x613))+((j0*x611))+((py*x612)));
705 evalcond[3]=((-4.14554770690028e-9)+(((-1.0)*x612*x613))+((j0*x612))+(((-1.0)*x615)));
706 evalcond[4]=((-2.9018833948302e-9)+((j0*x614))+(((-0.7)*x615))+(((-1.0)*pz*x614)));
713 rotationfunction0(solutions);
719 if( bgotonextstatement )
721 bool bgotonextstatement =
true;
724 IkReal x616=
IKcos(pz);
725 IkReal x617=
IKsin(pz);
726 if((((-1.0)*(py*py))) < -0.00001)
728 IkReal x618=
IKsqrt(((-1.0)*(py*py)));
729 IkReal x619=
IKcos(x618);
730 IkReal x620=
IKsin(x618);
731 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
733 IkReal gconst0=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
734 IkReal gconst1=(((x616*x620))+((x617*x619)));
735 IkReal gconst2=(((x616*x619))+(((-1.0)*x617*x620)));
736 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
738 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
739 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
741 bgotonextstatement=
false;
744 IkReal x621=
IKcos(pz);
745 IkReal x622=
IKsin(pz);
747 IkReal x624=
IKcos(x623);
748 IkReal x625=
IKsin(x623);
749 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
751 IkReal x626=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
759 IkReal gconst1=(((x621*x625))+((x622*x624)));
760 IkReal gconst2=((((-1.0)*x622*x625))+((x621*x624)));
761 j1eval[0]=
IKabs(((-6.36363636363636)+cj3+(((18.1818181818182)*py))+(((20.0)*sj3))));
762 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
768 IkReal op[2+1], zeror[2];
770 IkReal x627=((18.1818181818182)*py);
771 IkReal x628=((20.0)*sj3);
772 op[0]=((-6.36363636363636)+cj3+x627+x628);
774 op[2]=((6.36363636363636)+(((-1.0)*x628))+x627+(((-1.0)*cj3)));
775 polyroots2(op,zeror,numroots);
776 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
777 int numsolutions = 0;
778 for(
int ij1 = 0; ij1 < numroots; ++ij1)
780 IkReal htj1 = zeror[ij1];
781 tempj1array[0]=((2.0)*(
atan(htj1)));
782 for(
int kj1 = 0; kj1 < 1; ++kj1)
784 j1array[numsolutions] = tempj1array[kj1];
785 if( j1array[numsolutions] >
IKPI )
787 j1array[numsolutions]-=
IK2PI;
789 else if( j1array[numsolutions] < -
IKPI )
791 j1array[numsolutions]+=
IK2PI;
793 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
794 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
798 bool j1valid[2]={
true,
true};
800 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
806 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
809 _ij1[0] = ij1; _ij1[1] = -1;
810 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
814 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
817 rotationfunction0(solutions);
826 if( bgotonextstatement )
828 bool bgotonextstatement =
true;
831 IkReal x629=
IKcos(pz);
832 IkReal x630=
IKsin(pz);
833 if((((-1.0)*(py*py))) < -0.00001)
835 IkReal x631=
IKsqrt(((-1.0)*(py*py)));
836 IkReal x632=
IKcos(x631);
837 IkReal x633=
IKsin(x631);
838 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
840 IkReal gconst3=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
841 IkReal gconst4=((((-1.0)*x629*x633))+((x630*x632)));
842 IkReal gconst5=(((x629*x632))+((x630*x633)));
843 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
845 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
846 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
848 bgotonextstatement=
false;
851 IkReal x634=
IKcos(pz);
852 IkReal x635=
IKsin(pz);
854 IkReal x637=
IKcos(x636);
855 IkReal x638=
IKsin(x636);
856 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
858 IkReal x639=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
866 IkReal gconst4=((((-1.0)*x634*x638))+((x635*x637)));
867 IkReal gconst5=(((x634*x637))+((x635*x638)));
868 j1eval[0]=
IKabs(((-6.36363636363636)+cj3+(((18.1818181818182)*py))+(((20.0)*sj3))));
869 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
875 IkReal op[2+1], zeror[2];
877 IkReal x640=((18.1818181818182)*py);
878 IkReal x641=((20.0)*sj3);
879 op[0]=((-6.36363636363636)+cj3+x641+x640);
881 op[2]=((6.36363636363636)+(((-1.0)*x641))+x640+(((-1.0)*cj3)));
882 polyroots2(op,zeror,numroots);
883 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
884 int numsolutions = 0;
885 for(
int ij1 = 0; ij1 < numroots; ++ij1)
887 IkReal htj1 = zeror[ij1];
888 tempj1array[0]=((2.0)*(
atan(htj1)));
889 for(
int kj1 = 0; kj1 < 1; ++kj1)
891 j1array[numsolutions] = tempj1array[kj1];
892 if( j1array[numsolutions] >
IKPI )
894 j1array[numsolutions]-=
IK2PI;
896 else if( j1array[numsolutions] < -
IKPI )
898 j1array[numsolutions]+=
IK2PI;
900 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
901 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
905 bool j1valid[2]={
true,
true};
907 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
913 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
916 _ij1[0] = ij1; _ij1[1] = -1;
917 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
921 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
924 rotationfunction0(solutions);
933 if( bgotonextstatement )
935 bool bgotonextstatement =
true;
940 bgotonextstatement=
false;
945 if( bgotonextstatement )
957 IkReal j1array[1], cj1array[1], sj1array[1];
958 bool j1valid[1]={
false};
960 IkReal x642=((0.055)*cj3);
961 IkReal x643=((1.1)*sj3);
972 j1array[0]=
IKatan2(((x644.
value)*(((((-1.0)*pz))+j0))), ((x645.
value)*(((((-1.0)*j0*py))+((py*pz))))));
973 sj1array[0]=
IKsin(j1array[0]);
974 cj1array[0]=
IKcos(j1array[0]);
975 if( j1array[0] >
IKPI )
979 else if( j1array[0] < -
IKPI )
983 for(
int ij1 = 0; ij1 < 1; ++ij1)
989 _ij1[0] = ij1; _ij1[1] = -1;
990 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
994 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
997 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1000 IkReal x646=
IKcos(j1);
1001 IkReal x647=
IKsin(j1);
1002 IkReal x648=((1.1)*sj3);
1003 IkReal x649=((0.055)*cj3);
1004 IkReal x650=(j0*x647);
1005 IkReal x651=(pz*x647);
1006 IkReal x652=((1.0)*x646);
1007 evalcond[0]=(((j0*x646))+((py*x647))+(((-1.0)*pz*x652)));
1008 evalcond[1]=((((-1.0)*x646*x648))+(((-1.0)*x646*x649))+(((0.35)*x646))+py);
1009 evalcond[2]=((((-1.0)*x647*x648))+(((-1.0)*x647*x649))+(((0.35)*x647))+pz+(((-1.0)*j0)));
1010 evalcond[3]=((-0.35)+(((-1.0)*x651))+x649+x648+x650+(((-1.0)*py*x652)));
1011 evalcond[4]=((-0.245)+(((0.7)*x650))+(((-0.7)*x651))+(((0.77)*sj3))+(((0.0385)*cj3))+(((-0.7)*py*x646)));
1018 rotationfunction0(solutions);
1029 IkReal j1array[1], cj1array[1], sj1array[1];
1030 bool j1valid[1]={
false};
1032 IkReal x653=((1.1)*sj3);
1033 IkReal x654=((0.055)*cj3);
1042 j1array[0]=((-1.5707963267949)+(x655.
value)+(((1.5707963267949)*(x656.
value))));
1043 sj1array[0]=
IKsin(j1array[0]);
1044 cj1array[0]=
IKcos(j1array[0]);
1045 if( j1array[0] >
IKPI )
1049 else if( j1array[0] < -
IKPI )
1050 { j1array[0]+=
IK2PI;
1053 for(
int ij1 = 0; ij1 < 1; ++ij1)
1059 _ij1[0] = ij1; _ij1[1] = -1;
1060 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1064 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1067 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1070 IkReal x657=
IKcos(j1);
1071 IkReal x658=
IKsin(j1);
1072 IkReal x659=((1.1)*sj3);
1073 IkReal x660=((0.055)*cj3);
1074 IkReal x661=(j0*x658);
1075 IkReal x662=(pz*x658);
1076 IkReal x663=((1.0)*x657);
1077 evalcond[0]=(((j0*x657))+((py*x658))+(((-1.0)*pz*x663)));
1078 evalcond[1]=((((-1.0)*x657*x659))+(((0.35)*x657))+(((-1.0)*x657*x660))+py);
1079 evalcond[2]=((((-1.0)*x658*x660))+(((0.35)*x658))+pz+(((-1.0)*j0))+(((-1.0)*x658*x659)));
1080 evalcond[3]=((-0.35)+(((-1.0)*x662))+(((-1.0)*py*x663))+x659+x661+x660);
1081 evalcond[4]=((-0.245)+(((-0.7)*x662))+(((0.77)*sj3))+(((0.0385)*cj3))+(((-0.7)*py*x657))+(((0.7)*x661)));
1088 rotationfunction0(solutions);
1099 IkReal j1array[1], cj1array[1], sj1array[1];
1100 bool j1valid[1]={
false};
1110 j1array[0]=((-1.5707963267949)+(x664.
value)+(((1.5707963267949)*(x665.
value))));
1111 sj1array[0]=
IKsin(j1array[0]);
1112 cj1array[0]=
IKcos(j1array[0]);
1113 if( j1array[0] >
IKPI )
1117 else if( j1array[0] < -
IKPI )
1118 { j1array[0]+=
IK2PI;
1121 for(
int ij1 = 0; ij1 < 1; ++ij1)
1127 _ij1[0] = ij1; _ij1[1] = -1;
1128 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1132 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1135 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1138 IkReal x666=
IKcos(j1);
1139 IkReal x667=
IKsin(j1);
1140 IkReal x668=((1.1)*sj3);
1141 IkReal x669=((0.055)*cj3);
1142 IkReal x670=(j0*x667);
1143 IkReal x671=(pz*x667);
1144 IkReal x672=((1.0)*x666);
1145 evalcond[0]=(((j0*x666))+(((-1.0)*pz*x672))+((py*x667)));
1146 evalcond[1]=(py+(((0.35)*x666))+(((-1.0)*x666*x668))+(((-1.0)*x666*x669)));
1147 evalcond[2]=(pz+(((0.35)*x667))+(((-1.0)*j0))+(((-1.0)*x667*x669))+(((-1.0)*x667*x668)));
1148 evalcond[3]=((-0.35)+(((-1.0)*py*x672))+x669+x668+x670+(((-1.0)*x671)));
1149 evalcond[4]=((-0.245)+(((-0.7)*x671))+(((0.77)*sj3))+(((0.7)*x670))+(((0.0385)*cj3))+(((-0.7)*py*x666)));
1156 rotationfunction0(solutions);
1166 if( bgotonextstatement )
1168 bool bgotonextstatement =
true;
1171 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j2)))), 6.28318530717959)));
1172 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1174 bgotonextstatement=
false;
1179 j2=-1.5707963267949;
1180 j1eval[0]=((6.36363636363636)+cj3+(((20.0)*sj3)));
1181 j1eval[1]=
IKsign(((0.35)+(((1.1)*sj3))+(((0.055)*cj3))));
1182 j1eval[2]=((
IKabs(py))+(
IKabs(((((-1.0)*pz))+j0))));
1183 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1189 j2=-1.5707963267949;
1190 IkReal x673=((0.055)*cj3);
1191 IkReal x674=((1.1)*sj3);
1192 IkReal x675=((j0*j0)+(((-1.0)*(px*px)))+pp+(((-2.0)*j0*pz)));
1194 j1eval[1]=((((0.005)*(
IKabs(((((-70.0)*py))+(((-220.0)*py*sj3))+(((-11.0)*cj3*py)))))))+(
IKabs(((((-0.35)*pz))+(((0.35)*j0))+((j0*x674))+((j0*x673))+(((-1.0)*pz*x674))+(((-1.0)*pz*x673))))));
1196 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1202 j2=-1.5707963267949;
1203 IkReal x676=((20.0)*sj3);
1204 j1eval[0]=((6.36363636363636)+cj3+x676);
1205 j1eval[1]=((((6.36363636363636)*j0))+(((-1.0)*cj3*pz))+((cj3*j0))+((j0*x676))+(((-1.0)*pz*x676))+(((-6.36363636363636)*pz)));
1206 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
1210 bool bgotonextstatement =
true;
1213 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((0.373350695000068)+j3)))), 6.28318530717959)));
1214 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1216 bgotonextstatement=
false;
1218 IkReal j1array[1], cj1array[1], sj1array[1];
1219 bool j1valid[1]={
false};
1223 j1array[0]=
IKatan2(((((-1135096109.82221)*pz))+(((1135096109.82221)*j0))), ((-1135096109.82221)*py));
1224 sj1array[0]=
IKsin(j1array[0]);
1225 cj1array[0]=
IKcos(j1array[0]);
1226 if( j1array[0] >
IKPI )
1230 else if( j1array[0] < -
IKPI )
1231 { j1array[0]+=
IK2PI;
1234 for(
int ij1 = 0; ij1 < 1; ++ij1)
1240 _ij1[0] = ij1; _ij1[1] = -1;
1241 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1245 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1248 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1251 IkReal x677=
IKcos(j1);
1252 IkReal x678=
IKsin(j1);
1253 IkReal x679=((1.0)*j0);
1254 IkReal x680=(py*x677);
1255 IkReal x681=((0.7)*x678);
1256 evalcond[0]=((((8.80982668645239e-10)*x677))+py);
1257 evalcond[1]=((((8.80982668645239e-10)*x678))+pz+(((-1.0)*x679)));
1258 evalcond[2]=(((py*x678))+((j0*x677))+(((-1.0)*pz*x677)));
1259 evalcond[3]=((8.80982668645239e-10)+((pz*x678))+(((-1.0)*x678*x679))+x680);
1260 evalcond[4]=((-6.16687868051667e-10)+((j0*x681))+(((-1.0)*pz*x681))+(((-0.7)*x680)));
1267 rotationfunction0(solutions);
1273 if( bgotonextstatement )
1275 bool bgotonextstatement =
true;
1278 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.41502655714598)+j3)))), 6.28318530717959)));
1279 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1281 bgotonextstatement=
false;
1283 IkReal j1array[1], cj1array[1], sj1array[1];
1284 bool j1valid[1]={
false};
1288 j1array[0]=
IKatan2(((((-241222649.141269)*pz))+(((241222649.141269)*j0))), ((-241222649.141269)*py));
1289 sj1array[0]=
IKsin(j1array[0]);
1290 cj1array[0]=
IKcos(j1array[0]);
1291 if( j1array[0] >
IKPI )
1295 else if( j1array[0] < -
IKPI )
1296 { j1array[0]+=
IK2PI;
1299 for(
int ij1 = 0; ij1 < 1; ++ij1)
1305 _ij1[0] = ij1; _ij1[1] = -1;
1306 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1310 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1313 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1316 IkReal x682=
IKcos(j1);
1317 IkReal x683=
IKsin(j1);
1318 IkReal x684=((1.0)*j0);
1319 IkReal x685=(py*x682);
1320 IkReal x686=((0.7)*x683);
1321 evalcond[0]=((((4.14554770690028e-9)*x682))+py);
1322 evalcond[1]=((((-1.0)*x684))+(((4.14554770690028e-9)*x683))+pz);
1323 evalcond[2]=(((j0*x682))+(((-1.0)*pz*x682))+((py*x683)));
1324 evalcond[3]=((4.14554770690028e-9)+x685+(((-1.0)*x683*x684))+((pz*x683)));
1325 evalcond[4]=((-2.9018833948302e-9)+((j0*x686))+(((-1.0)*pz*x686))+(((-0.7)*x685)));
1332 rotationfunction0(solutions);
1338 if( bgotonextstatement )
1340 bool bgotonextstatement =
true;
1343 IkReal x687=
IKcos(pz);
1344 IkReal x688=
IKsin(pz);
1345 if((((-1.0)*(py*py))) < -0.00001)
1347 IkReal x689=
IKsqrt(((-1.0)*(py*py)));
1348 IkReal x690=
IKcos(x689);
1349 IkReal x691=
IKsin(x689);
1350 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1352 IkReal gconst6=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1353 IkReal gconst7=(((x688*x690))+((x687*x691)));
1354 IkReal gconst8=(((x687*x690))+(((-1.0)*x688*x691)));
1355 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1357 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
1358 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1360 bgotonextstatement=
false;
1363 IkReal x692=
IKcos(pz);
1364 IkReal x693=
IKsin(pz);
1366 IkReal x695=
IKcos(x694);
1367 IkReal x696=
IKsin(x694);
1368 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1370 IkReal x697=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1373 j2=-1.5707963267949;
1377 IkReal gconst6=x697;
1378 IkReal gconst7=(((x692*x696))+((x693*x695)));
1379 IkReal gconst8=((((-1.0)*x693*x696))+((x692*x695)));
1380 j1eval[0]=
IKabs(((-6.36363636363636)+(((-20.0)*sj3))+(((18.1818181818182)*py))+(((-1.0)*cj3))));
1381 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
1387 IkReal op[2+1], zeror[2];
1389 IkReal x698=((18.1818181818182)*py);
1390 IkReal x699=((20.0)*sj3);
1391 op[0]=((-6.36363636363636)+x698+(((-1.0)*x699))+(((-1.0)*cj3)));
1393 op[2]=((6.36363636363636)+cj3+x698+x699);
1394 polyroots2(op,zeror,numroots);
1395 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1396 int numsolutions = 0;
1397 for(
int ij1 = 0; ij1 < numroots; ++ij1)
1399 IkReal htj1 = zeror[ij1];
1400 tempj1array[0]=((2.0)*(
atan(htj1)));
1401 for(
int kj1 = 0; kj1 < 1; ++kj1)
1403 j1array[numsolutions] = tempj1array[kj1];
1404 if( j1array[numsolutions] >
IKPI )
1406 j1array[numsolutions]-=
IK2PI;
1408 else if( j1array[numsolutions] < -
IKPI )
1410 j1array[numsolutions]+=
IK2PI;
1412 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
1413 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
1417 bool j1valid[2]={
true,
true};
1419 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
1425 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1428 _ij1[0] = ij1; _ij1[1] = -1;
1429 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
1433 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1436 rotationfunction0(solutions);
1445 if( bgotonextstatement )
1447 bool bgotonextstatement =
true;
1450 IkReal x700=
IKcos(pz);
1451 IkReal x701=
IKsin(pz);
1452 if((((-1.0)*(py*py))) < -0.00001)
1454 IkReal x702=
IKsqrt(((-1.0)*(py*py)));
1455 IkReal x703=
IKcos(x702);
1456 IkReal x704=
IKsin(x702);
1457 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1459 IkReal gconst9=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1460 IkReal gconst10=((((-1.0)*x700*x704))+((x701*x703)));
1461 IkReal gconst11=(((x701*x704))+((x700*x703)));
1462 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1464 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
1465 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1467 bgotonextstatement=
false;
1470 IkReal x705=
IKcos(pz);
1471 IkReal x706=
IKsin(pz);
1473 IkReal x708=
IKcos(x707);
1474 IkReal x709=
IKsin(x707);
1475 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1477 IkReal x710=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1480 j2=-1.5707963267949;
1484 IkReal gconst9=x710;
1485 IkReal gconst10=(((x706*x708))+(((-1.0)*x705*x709)));
1486 IkReal gconst11=(((x706*x709))+((x705*x708)));
1487 j1eval[0]=
IKabs(((-6.36363636363636)+(((-20.0)*sj3))+(((18.1818181818182)*py))+(((-1.0)*cj3))));
1488 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
1494 IkReal op[2+1], zeror[2];
1496 IkReal x711=((18.1818181818182)*py);
1497 IkReal x712=((20.0)*sj3);
1498 op[0]=((-6.36363636363636)+(((-1.0)*x712))+x711+(((-1.0)*cj3)));
1500 op[2]=((6.36363636363636)+cj3+x711+x712);
1501 polyroots2(op,zeror,numroots);
1502 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1503 int numsolutions = 0;
1504 for(
int ij1 = 0; ij1 < numroots; ++ij1)
1506 IkReal htj1 = zeror[ij1];
1507 tempj1array[0]=((2.0)*(
atan(htj1)));
1508 for(
int kj1 = 0; kj1 < 1; ++kj1)
1510 j1array[numsolutions] = tempj1array[kj1];
1511 if( j1array[numsolutions] >
IKPI )
1513 j1array[numsolutions]-=
IK2PI;
1515 else if( j1array[numsolutions] < -
IKPI )
1517 j1array[numsolutions]+=
IK2PI;
1519 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
1520 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
1524 bool j1valid[2]={
true,
true};
1526 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
1532 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1535 _ij1[0] = ij1; _ij1[1] = -1;
1536 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
1540 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1543 rotationfunction0(solutions);
1552 if( bgotonextstatement )
1554 bool bgotonextstatement =
true;
1559 bgotonextstatement=
false;
1564 if( bgotonextstatement )
1576 IkReal j1array[1], cj1array[1], sj1array[1];
1577 bool j1valid[1]={
false};
1579 IkReal x713=((0.055)*cj3);
1580 IkReal x714=((1.1)*sj3);
1591 j1array[0]=
IKatan2(((x715.
value)*(((((-1.0)*pz))+j0))), ((x716.
value)*(((((-1.0)*j0*py))+((py*pz))))));
1592 sj1array[0]=
IKsin(j1array[0]);
1593 cj1array[0]=
IKcos(j1array[0]);
1594 if( j1array[0] >
IKPI )
1598 else if( j1array[0] < -
IKPI )
1599 { j1array[0]+=
IK2PI;
1602 for(
int ij1 = 0; ij1 < 1; ++ij1)
1608 _ij1[0] = ij1; _ij1[1] = -1;
1609 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1613 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1616 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1619 IkReal x717=
IKcos(j1);
1620 IkReal x718=
IKsin(j1);
1621 IkReal x719=((1.1)*sj3);
1622 IkReal x720=((0.055)*cj3);
1623 IkReal x721=((1.0)*j0);
1624 IkReal x722=(py*x717);
1625 IkReal x723=(pz*x718);
1626 evalcond[0]=(((j0*x717))+(((-1.0)*pz*x717))+((py*x718)));
1627 evalcond[1]=(((x717*x720))+((x717*x719))+py+(((0.35)*x717)));
1628 evalcond[2]=((((-1.0)*x721))+pz+((x718*x720))+((x718*x719))+(((0.35)*x718)));
1629 evalcond[3]=((0.35)+x719+x720+x722+x723+(((-1.0)*x718*x721)));
1630 evalcond[4]=((-0.245)+(((-0.7)*x723))+(((-0.7)*x722))+(((0.7)*j0*x718))+(((-0.0385)*cj3))+(((-0.77)*sj3)));
1637 rotationfunction0(solutions);
1648 IkReal j1array[1], cj1array[1], sj1array[1];
1649 bool j1valid[1]={
false};
1651 IkReal x724=((1.1)*sj3);
1652 IkReal x725=((0.055)*cj3);
1661 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x726.
value)))+(x727.
value));
1662 sj1array[0]=
IKsin(j1array[0]);
1663 cj1array[0]=
IKcos(j1array[0]);
1664 if( j1array[0] >
IKPI )
1668 else if( j1array[0] < -
IKPI )
1669 { j1array[0]+=
IK2PI;
1672 for(
int ij1 = 0; ij1 < 1; ++ij1)
1678 _ij1[0] = ij1; _ij1[1] = -1;
1679 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1683 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1686 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1689 IkReal x728=
IKcos(j1);
1690 IkReal x729=
IKsin(j1);
1691 IkReal x730=((1.1)*sj3);
1692 IkReal x731=((0.055)*cj3);
1693 IkReal x732=((1.0)*j0);
1694 IkReal x733=(py*x728);
1695 IkReal x734=(pz*x729);
1696 evalcond[0]=(((py*x729))+((j0*x728))+(((-1.0)*pz*x728)));
1697 evalcond[1]=(((x728*x731))+((x728*x730))+py+(((0.35)*x728)));
1698 evalcond[2]=(((x729*x730))+((x729*x731))+(((-1.0)*x732))+pz+(((0.35)*x729)));
1699 evalcond[3]=((0.35)+x734+x733+x731+x730+(((-1.0)*x729*x732)));
1700 evalcond[4]=((-0.245)+(((-0.7)*x733))+(((-0.7)*x734))+(((-0.0385)*cj3))+(((0.7)*j0*x729))+(((-0.77)*sj3)));
1707 rotationfunction0(solutions);
1718 IkReal j1array[1], cj1array[1], sj1array[1];
1719 bool j1valid[1]={
false};
1729 j1array[0]=((-1.5707963267949)+(x735.
value)+(((1.5707963267949)*(x736.
value))));
1730 sj1array[0]=
IKsin(j1array[0]);
1731 cj1array[0]=
IKcos(j1array[0]);
1732 if( j1array[0] >
IKPI )
1736 else if( j1array[0] < -
IKPI )
1737 { j1array[0]+=
IK2PI;
1740 for(
int ij1 = 0; ij1 < 1; ++ij1)
1746 _ij1[0] = ij1; _ij1[1] = -1;
1747 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
1751 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1754 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1757 IkReal x737=
IKcos(j1);
1758 IkReal x738=
IKsin(j1);
1759 IkReal x739=((1.1)*sj3);
1760 IkReal x740=((0.055)*cj3);
1761 IkReal x741=((1.0)*j0);
1762 IkReal x742=(py*x737);
1763 IkReal x743=(pz*x738);
1764 evalcond[0]=((((-1.0)*pz*x737))+((py*x738))+((j0*x737)));
1765 evalcond[1]=(py+((x737*x740))+(((0.35)*x737))+((x737*x739)));
1766 evalcond[2]=(((x738*x740))+pz+(((0.35)*x738))+((x738*x739))+(((-1.0)*x741)));
1767 evalcond[3]=((0.35)+(((-1.0)*x738*x741))+x739+x742+x743+x740);
1768 evalcond[4]=((-0.245)+(((0.7)*j0*x738))+(((-0.7)*x743))+(((-0.7)*x742))+(((-0.0385)*cj3))+(((-0.77)*sj3)));
1775 rotationfunction0(solutions);
1785 if( bgotonextstatement )
1787 bool bgotonextstatement =
true;
1790 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j2))), 6.28318530717959)));
1791 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1793 bgotonextstatement=
false;
1799 j1eval[0]=((29.0909090909091)+(((-1.0)*sj3))+(((20.0)*cj3)));
1800 j1eval[1]=
IKsign(((1.6)+(((1.1)*cj3))+(((-0.055)*sj3))));
1801 j1eval[2]=((
IKabs(py))+(
IKabs(((((-1.0)*pz))+j0))));
1802 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1809 IkReal x744=((1.1)*cj3);
1810 IkReal x745=((0.055)*sj3);
1811 IkReal x746=((j0*j0)+(((-1.0)*(px*px)))+pp+(((-2.0)*j0*pz)));
1813 j1eval[1]=((((0.005)*(
IKabs(((((-220.0)*cj3*py))+(((-320.0)*py))+(((11.0)*py*sj3)))))))+(
IKabs((((j0*x744))+(((-1.0)*j0*x745))+(((-1.0)*pz*x744))+(((-1.6)*pz))+(((1.6)*j0))+((pz*x745))))));
1815 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
1822 IkReal x747=((20.0)*cj3);
1823 IkReal x748=((1.0)*sj3);
1824 j1eval[0]=((29.0909090909091)+x747+(((-1.0)*x748)));
1825 j1eval[1]=((((29.0909090909091)*j0))+(((-29.0909090909091)*pz))+((j0*x747))+(((-1.0)*j0*x748))+(((-1.0)*pz*x747))+((pz*sj3)));
1826 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
1830 bool bgotonextstatement =
true;
1833 IkReal x749=
IKcos(pz);
1834 IkReal x750=
IKsin(pz);
1835 if((((-1.0)*(py*py))) < -0.00001)
1837 IkReal x751=
IKsqrt(((-1.0)*(py*py)));
1838 IkReal x752=
IKcos(x751);
1839 IkReal x753=
IKsin(x751);
1840 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1842 IkReal gconst12=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1843 IkReal gconst13=(((x749*x753))+((x750*x752)));
1844 IkReal gconst14=((((-1.0)*x750*x753))+((x749*x752)));
1845 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1847 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
1848 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1850 bgotonextstatement=
false;
1853 IkReal x754=
IKcos(pz);
1854 IkReal x755=
IKsin(pz);
1856 IkReal x757=
IKcos(x756);
1857 IkReal x758=
IKsin(x756);
1858 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1860 IkReal x759=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
1867 IkReal gconst12=x759;
1868 IkReal gconst13=(((x755*x757))+((x754*x758)));
1869 IkReal gconst14=((((-1.0)*x755*x758))+((x754*x757)));
1870 j1eval[0]=
IKabs(((-29.0909090909091)+sj3+(((18.1818181818182)*py))+(((-20.0)*cj3))));
1871 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
1877 IkReal op[2+1], zeror[2];
1879 IkReal x760=((18.1818181818182)*py);
1880 IkReal x761=((20.0)*cj3);
1881 op[0]=((-29.0909090909091)+sj3+(((-1.0)*x761))+x760);
1883 op[2]=((29.0909090909091)+(((-1.0)*sj3))+x760+x761);
1884 polyroots2(op,zeror,numroots);
1885 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1886 int numsolutions = 0;
1887 for(
int ij1 = 0; ij1 < numroots; ++ij1)
1889 IkReal htj1 = zeror[ij1];
1890 tempj1array[0]=((2.0)*(
atan(htj1)));
1891 for(
int kj1 = 0; kj1 < 1; ++kj1)
1893 j1array[numsolutions] = tempj1array[kj1];
1894 if( j1array[numsolutions] >
IKPI )
1896 j1array[numsolutions]-=
IK2PI;
1898 else if( j1array[numsolutions] < -
IKPI )
1900 j1array[numsolutions]+=
IK2PI;
1902 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
1903 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
1907 bool j1valid[2]={
true,
true};
1909 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
1915 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
1918 _ij1[0] = ij1; _ij1[1] = -1;
1919 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
1923 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
1926 rotationfunction0(solutions);
1935 if( bgotonextstatement )
1937 bool bgotonextstatement =
true;
1940 IkReal x762=
IKcos(pz);
1941 IkReal x763=
IKsin(pz);
1942 if((((-1.0)*(py*py))) < -0.00001)
1944 IkReal x764=
IKsqrt(((-1.0)*(py*py)));
1945 IkReal x765=
IKcos(x764);
1946 IkReal x766=
IKsin(x764);
1947 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1949 IkReal gconst15=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1950 IkReal gconst16=(((x763*x765))+(((-1.0)*x762*x766)));
1951 IkReal gconst17=(((x763*x766))+((x762*x765)));
1952 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1954 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
1955 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
1957 bgotonextstatement=
false;
1960 IkReal x767=
IKcos(pz);
1961 IkReal x768=
IKsin(pz);
1963 IkReal x770=
IKcos(x769);
1964 IkReal x771=
IKsin(x769);
1965 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
1967 IkReal x772=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
1974 IkReal gconst15=x772;
1975 IkReal gconst16=((((-1.0)*x767*x771))+((x768*x770)));
1976 IkReal gconst17=(((x767*x770))+((x768*x771)));
1977 j1eval[0]=
IKabs(((-29.0909090909091)+sj3+(((18.1818181818182)*py))+(((-20.0)*cj3))));
1978 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
1984 IkReal op[2+1], zeror[2];
1986 IkReal x773=((18.1818181818182)*py);
1987 IkReal x774=((20.0)*cj3);
1988 op[0]=((-29.0909090909091)+sj3+x773+(((-1.0)*x774)));
1990 op[2]=((29.0909090909091)+(((-1.0)*sj3))+x773+x774);
1991 polyroots2(op,zeror,numroots);
1992 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
1993 int numsolutions = 0;
1994 for(
int ij1 = 0; ij1 < numroots; ++ij1)
1996 IkReal htj1 = zeror[ij1];
1997 tempj1array[0]=((2.0)*(
atan(htj1)));
1998 for(
int kj1 = 0; kj1 < 1; ++kj1)
2000 j1array[numsolutions] = tempj1array[kj1];
2001 if( j1array[numsolutions] >
IKPI )
2003 j1array[numsolutions]-=
IK2PI;
2005 else if( j1array[numsolutions] < -
IKPI )
2007 j1array[numsolutions]+=
IK2PI;
2009 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
2010 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
2014 bool j1valid[2]={
true,
true};
2016 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
2022 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2025 _ij1[0] = ij1; _ij1[1] = -1;
2026 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
2030 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2033 rotationfunction0(solutions);
2042 if( bgotonextstatement )
2044 bool bgotonextstatement =
true;
2049 bgotonextstatement=
false;
2054 if( bgotonextstatement )
2064 IkReal j1array[1], cj1array[1], sj1array[1];
2065 bool j1valid[1]={
false};
2067 IkReal x775=((1.1)*cj3);
2068 IkReal x776=((0.055)*sj3);
2079 j1array[0]=
IKatan2(((x777.
value)*(((((-1.0)*pz))+j0))), ((x778.
value)*(((((-1.0)*j0*py))+((py*pz))))));
2080 sj1array[0]=
IKsin(j1array[0]);
2081 cj1array[0]=
IKcos(j1array[0]);
2082 if( j1array[0] >
IKPI )
2086 else if( j1array[0] < -
IKPI )
2087 { j1array[0]+=
IK2PI;
2090 for(
int ij1 = 0; ij1 < 1; ++ij1)
2096 _ij1[0] = ij1; _ij1[1] = -1;
2097 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2101 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2104 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2107 IkReal x779=
IKcos(j1);
2108 IkReal x780=
IKsin(j1);
2109 IkReal x781=((1.0)*j0);
2110 IkReal x782=((0.055)*sj3);
2111 IkReal x783=((1.1)*cj3);
2112 IkReal x784=(pz*x780);
2113 IkReal x785=(py*x779);
2114 evalcond[0]=((((-1.0)*pz*x779))+((py*x780))+((j0*x779)));
2115 evalcond[1]=(((x779*x783))+py+(((1.6)*x779))+(((-1.0)*x779*x782)));
2116 evalcond[2]=((((-1.0)*x781))+(((1.6)*x780))+pz+((x780*x783))+(((-1.0)*x780*x782)));
2117 evalcond[3]=((1.6)+(((-1.0)*x782))+x783+x784+x785+(((-1.0)*x780*x781)));
2118 evalcond[4]=((-5.12)+(((3.2)*j0*x780))+(((-3.2)*x784))+(((-3.2)*x785))+(((-3.52)*cj3))+(((0.176)*sj3)));
2125 rotationfunction0(solutions);
2136 IkReal j1array[1], cj1array[1], sj1array[1];
2137 bool j1valid[1]={
false};
2139 IkReal x786=((1.1)*cj3);
2140 IkReal x787=((0.055)*sj3);
2149 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x788.
value)))+(x789.
value));
2150 sj1array[0]=
IKsin(j1array[0]);
2151 cj1array[0]=
IKcos(j1array[0]);
2152 if( j1array[0] >
IKPI )
2156 else if( j1array[0] < -
IKPI )
2157 { j1array[0]+=
IK2PI;
2160 for(
int ij1 = 0; ij1 < 1; ++ij1)
2166 _ij1[0] = ij1; _ij1[1] = -1;
2167 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2171 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2174 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2177 IkReal x790=
IKcos(j1);
2178 IkReal x791=
IKsin(j1);
2179 IkReal x792=((1.0)*j0);
2180 IkReal x793=((0.055)*sj3);
2181 IkReal x794=((1.1)*cj3);
2182 IkReal x795=(pz*x791);
2183 IkReal x796=(py*x790);
2184 evalcond[0]=(((j0*x790))+((py*x791))+(((-1.0)*pz*x790)));
2185 evalcond[1]=((((1.6)*x790))+((x790*x794))+(((-1.0)*x790*x793))+py);
2186 evalcond[2]=((((-1.0)*x792))+(((1.6)*x791))+(((-1.0)*x791*x793))+pz+((x791*x794)));
2187 evalcond[3]=((1.6)+(((-1.0)*x793))+(((-1.0)*x791*x792))+x795+x794+x796);
2188 evalcond[4]=((-5.12)+(((3.2)*j0*x791))+(((-3.2)*x795))+(((-3.2)*x796))+(((-3.52)*cj3))+(((0.176)*sj3)));
2195 rotationfunction0(solutions);
2206 IkReal j1array[1], cj1array[1], sj1array[1];
2207 bool j1valid[1]={
false};
2217 j1array[0]=((-1.5707963267949)+(x797.
value)+(((1.5707963267949)*(x798.
value))));
2218 sj1array[0]=
IKsin(j1array[0]);
2219 cj1array[0]=
IKcos(j1array[0]);
2220 if( j1array[0] >
IKPI )
2224 else if( j1array[0] < -
IKPI )
2225 { j1array[0]+=
IK2PI;
2228 for(
int ij1 = 0; ij1 < 1; ++ij1)
2234 _ij1[0] = ij1; _ij1[1] = -1;
2235 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2239 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2242 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2245 IkReal x799=
IKcos(j1);
2246 IkReal x800=
IKsin(j1);
2247 IkReal x801=((1.0)*j0);
2248 IkReal x802=((0.055)*sj3);
2249 IkReal x803=((1.1)*cj3);
2250 IkReal x804=(pz*x800);
2251 IkReal x805=(py*x799);
2252 evalcond[0]=(((j0*x799))+(((-1.0)*pz*x799))+((py*x800)));
2253 evalcond[1]=((((-1.0)*x799*x802))+(((1.6)*x799))+((x799*x803))+py);
2254 evalcond[2]=((((1.6)*x800))+((x800*x803))+(((-1.0)*x800*x802))+pz+(((-1.0)*x801)));
2255 evalcond[3]=((1.6)+(((-1.0)*x800*x801))+x805+x804+x803+(((-1.0)*x802)));
2256 evalcond[4]=((-5.12)+(((-3.2)*x805))+(((-3.2)*x804))+(((-3.52)*cj3))+(((0.176)*sj3))+(((3.2)*j0*x800)));
2263 rotationfunction0(solutions);
2273 if( bgotonextstatement )
2275 bool bgotonextstatement =
true;
2278 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j2)))), 6.28318530717959)));
2279 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2281 bgotonextstatement=
false;
2286 j2=3.14159265358979;
2287 j1eval[0]=((-16.3636363636364)+sj3+(((-20.0)*cj3)));
2288 j1eval[1]=((
IKabs(py))+(
IKabs(((((-1.0)*pz))+j0))));
2289 j1eval[2]=
IKsign(((-0.9)+(((0.055)*sj3))+(((-1.1)*cj3))));
2290 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
2296 j2=3.14159265358979;
2297 IkReal x806=((1.1)*cj3);
2298 IkReal x807=((0.055)*sj3);
2299 IkReal x808=((px*px)+(((-1.0)*(j0*j0)))+(((-1.0)*pp))+(((2.0)*j0*pz)));
2302 j1eval[2]=((
IKabs(((((-1.0)*j0*x807))+((j0*x806))+(((0.9)*j0))+((pz*x807))+(((-0.9)*pz))+(((-1.0)*pz*x806)))))+(((0.005)*(
IKabs(((((-220.0)*cj3*py))+(((-180.0)*py))+(((11.0)*py*sj3))))))));
2303 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 ||
IKabs(j1eval[2]) < 0.0000010000000000 )
2309 j2=3.14159265358979;
2310 IkReal x809=((20.0)*cj3);
2311 j1eval[0]=((-16.3636363636364)+sj3+(((-1.0)*x809)));
2312 j1eval[1]=((((-1.0)*j0*x809))+(((16.3636363636364)*pz))+(((-16.3636363636364)*j0))+(((-1.0)*pz*sj3))+((j0*sj3))+((pz*x809)));
2313 if(
IKabs(j1eval[0]) < 0.0000010000000000 ||
IKabs(j1eval[1]) < 0.0000010000000000 )
2317 bool bgotonextstatement =
true;
2320 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-2.47730630290303)+j3)))), 6.28318530717959)));
2321 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2323 bgotonextstatement=
false;
2325 IkReal j1array[1], cj1array[1], sj1array[1];
2326 bool j1valid[1]={
false};
2330 j1array[0]=
IKatan2(((((886700339.955615)*j0))+(((-886700339.955615)*pz))), ((-886700339.955615)*py));
2331 sj1array[0]=
IKsin(j1array[0]);
2332 cj1array[0]=
IKcos(j1array[0]);
2333 if( j1array[0] >
IKPI )
2337 else if( j1array[0] < -
IKPI )
2338 { j1array[0]+=
IK2PI;
2341 for(
int ij1 = 0; ij1 < 1; ++ij1)
2347 _ij1[0] = ij1; _ij1[1] = -1;
2348 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2352 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2355 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2358 IkReal x810=
IKcos(j1);
2359 IkReal x811=
IKsin(j1);
2360 IkReal x812=((1.0)*pz);
2361 IkReal x813=(py*x810);
2362 IkReal x814=(j0*x811);
2363 evalcond[0]=((((1.12777671885189e-9)*x810))+py);
2364 evalcond[1]=((((1.12777671885189e-9)*x811))+pz+(((-1.0)*j0)));
2365 evalcond[2]=(((py*x811))+(((-1.0)*x810*x812))+((j0*x810)));
2366 evalcond[3]=((-1.12777671885189e-9)+(((-1.0)*x811*x812))+(((-1.0)*x813))+x814);
2367 evalcond[4]=((2.0299980939334e-9)+(((1.8)*x813))+(((1.8)*pz*x811))+(((-1.8)*x814)));
2374 rotationfunction0(solutions);
2380 if( bgotonextstatement )
2382 bool bgotonextstatement =
true;
2385 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.70596221283267)+j3)))), 6.28318530717959)));
2386 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2388 bgotonextstatement=
false;
2390 IkReal j1array[1], cj1array[1], sj1array[1];
2391 bool j1valid[1]={
false};
2395 j1array[0]=
IKatan2(((((-1333333333.33333)*j0))+(((1333333333.33333)*pz))), ((1333333333.33333)*py));
2396 sj1array[0]=
IKsin(j1array[0]);
2397 cj1array[0]=
IKcos(j1array[0]);
2398 if( j1array[0] >
IKPI )
2402 else if( j1array[0] < -
IKPI )
2403 { j1array[0]+=
IK2PI;
2406 for(
int ij1 = 0; ij1 < 1; ++ij1)
2412 _ij1[0] = ij1; _ij1[1] = -1;
2413 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2417 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2420 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2423 IkReal x815=
IKcos(j1);
2424 IkReal x816=
IKsin(j1);
2425 IkReal x817=((1.0)*pz);
2426 IkReal x818=(py*x815);
2427 IkReal x819=(j0*x816);
2428 evalcond[0]=(py+(((-7.5e-10)*x815)));
2429 evalcond[1]=(pz+(((-1.0)*j0))+(((-7.5e-10)*x816)));
2430 evalcond[2]=(((py*x816))+(((-1.0)*x815*x817))+((j0*x815)));
2431 evalcond[3]=((7.5e-10)+(((-1.0)*x816*x817))+(((-1.0)*x818))+x819);
2432 evalcond[4]=((-1.35e-9)+(((1.8)*x818))+(((1.8)*pz*x816))+(((-1.8)*x819)));
2439 rotationfunction0(solutions);
2445 if( bgotonextstatement )
2447 bool bgotonextstatement =
true;
2450 IkReal x820=
IKcos(pz);
2451 IkReal x821=
IKsin(pz);
2452 if((((-1.0)*(py*py))) < -0.00001)
2454 IkReal x822=
IKsqrt(((-1.0)*(py*py)));
2455 IkReal x823=
IKcos(x822);
2456 IkReal x824=
IKsin(x822);
2457 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2459 IkReal gconst18=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2460 IkReal gconst19=(((x820*x824))+((x821*x823)));
2461 IkReal gconst20=(((x820*x823))+(((-1.0)*x821*x824)));
2462 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2464 evalcond[0]=
IKabs(((((-1.0)*pz))+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))))+j0));
2465 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2467 bgotonextstatement=
false;
2470 IkReal x825=
IKcos(pz);
2471 IkReal x826=
IKsin(pz);
2473 IkReal x828=
IKcos(x827);
2474 IkReal x829=
IKsin(x827);
2475 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2477 IkReal x830=((
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+pz);
2480 j2=3.14159265358979;
2484 IkReal gconst18=x830;
2485 IkReal gconst19=(((x825*x829))+((x826*x828)));
2486 IkReal gconst20=(((x825*x828))+(((-1.0)*x826*x829)));
2487 j1eval[0]=
IKabs(((16.3636363636364)+(((-1.0)*sj3))+(((18.1818181818182)*py))+(((20.0)*cj3))));
2488 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
2494 IkReal op[2+1], zeror[2];
2496 IkReal x831=((18.1818181818182)*py);
2497 IkReal x832=((20.0)*cj3);
2498 op[0]=((16.3636363636364)+(((-1.0)*sj3))+x831+x832);
2500 op[2]=((-16.3636363636364)+sj3+(((-1.0)*x832))+x831);
2501 polyroots2(op,zeror,numroots);
2502 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
2503 int numsolutions = 0;
2504 for(
int ij1 = 0; ij1 < numroots; ++ij1)
2506 IkReal htj1 = zeror[ij1];
2507 tempj1array[0]=((2.0)*(
atan(htj1)));
2508 for(
int kj1 = 0; kj1 < 1; ++kj1)
2510 j1array[numsolutions] = tempj1array[kj1];
2511 if( j1array[numsolutions] >
IKPI )
2513 j1array[numsolutions]-=
IK2PI;
2515 else if( j1array[numsolutions] < -
IKPI )
2517 j1array[numsolutions]+=
IK2PI;
2519 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
2520 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
2524 bool j1valid[2]={
true,
true};
2526 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
2532 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2535 _ij1[0] = ij1; _ij1[1] = -1;
2536 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
2540 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2543 rotationfunction0(solutions);
2552 if( bgotonextstatement )
2554 bool bgotonextstatement =
true;
2557 IkReal x833=
IKcos(pz);
2558 IkReal x834=
IKsin(pz);
2559 if((((-1.0)*(py*py))) < -0.00001)
2561 IkReal x835=
IKsqrt(((-1.0)*(py*py)));
2562 IkReal x836=
IKcos(x835);
2563 IkReal x837=
IKsin(x835);
2564 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2566 IkReal gconst21=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2567 IkReal gconst22=(((x834*x836))+(((-1.0)*x833*x837)));
2568 IkReal gconst23=(((x833*x836))+((x834*x837)));
2569 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2571 evalcond[0]=
IKabs(((((-1.0)*pz))+(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz))))+j0));
2572 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
2574 bgotonextstatement=
false;
2577 IkReal x838=
IKcos(pz);
2578 IkReal x839=
IKsin(pz);
2580 IkReal x841=
IKcos(x840);
2581 IkReal x842=
IKsin(x840);
2582 if((((px*px)+(((-1.0)*pp))+(pz*pz))) < -0.00001)
2584 IkReal x843=(pz+(((-1.0)*(
IKsqrt(((px*px)+(((-1.0)*pp))+(pz*pz)))))));
2587 j2=3.14159265358979;
2591 IkReal gconst21=x843;
2592 IkReal gconst22=(((x839*x841))+(((-1.0)*x838*x842)));
2593 IkReal gconst23=(((x838*x841))+((x839*x842)));
2594 j1eval[0]=
IKabs(((16.3636363636364)+(((-1.0)*sj3))+(((18.1818181818182)*py))+(((20.0)*cj3))));
2595 if(
IKabs(j1eval[0]) < 0.0000000100000000 )
2601 IkReal op[2+1], zeror[2];
2603 IkReal x844=((18.1818181818182)*py);
2604 IkReal x845=((20.0)*cj3);
2605 op[0]=((16.3636363636364)+(((-1.0)*sj3))+x845+x844);
2607 op[2]=((-16.3636363636364)+sj3+(((-1.0)*x845))+x844);
2608 polyroots2(op,zeror,numroots);
2609 IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1];
2610 int numsolutions = 0;
2611 for(
int ij1 = 0; ij1 < numroots; ++ij1)
2613 IkReal htj1 = zeror[ij1];
2614 tempj1array[0]=((2.0)*(
atan(htj1)));
2615 for(
int kj1 = 0; kj1 < 1; ++kj1)
2617 j1array[numsolutions] = tempj1array[kj1];
2618 if( j1array[numsolutions] >
IKPI )
2620 j1array[numsolutions]-=
IK2PI;
2622 else if( j1array[numsolutions] < -
IKPI )
2624 j1array[numsolutions]+=
IK2PI;
2626 sj1array[numsolutions] =
IKsin(j1array[numsolutions]);
2627 cj1array[numsolutions] =
IKcos(j1array[numsolutions]);
2631 bool j1valid[2]={
true,
true};
2633 for(
int ij1 = 0; ij1 < numsolutions; ++ij1)
2639 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2642 _ij1[0] = ij1; _ij1[1] = -1;
2643 for(
int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
2647 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2650 rotationfunction0(solutions);
2659 if( bgotonextstatement )
2661 bool bgotonextstatement =
true;
2666 bgotonextstatement=
false;
2671 if( bgotonextstatement )
2683 IkReal j1array[1], cj1array[1], sj1array[1];
2684 bool j1valid[1]={
false};
2686 IkReal x846=((0.055)*sj3);
2687 IkReal x847=((1.1)*cj3);
2698 j1array[0]=
IKatan2(((x848.
value)*(((((-1.0)*pz))+j0))), ((x849.
value)*(((((-1.0)*j0*py))+((py*pz))))));
2699 sj1array[0]=
IKsin(j1array[0]);
2700 cj1array[0]=
IKcos(j1array[0]);
2701 if( j1array[0] >
IKPI )
2705 else if( j1array[0] < -
IKPI )
2706 { j1array[0]+=
IK2PI;
2709 for(
int ij1 = 0; ij1 < 1; ++ij1)
2715 _ij1[0] = ij1; _ij1[1] = -1;
2716 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2720 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2723 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2726 IkReal x850=
IKcos(j1);
2727 IkReal x851=
IKsin(j1);
2728 IkReal x852=((0.055)*sj3);
2729 IkReal x853=((1.1)*cj3);
2730 IkReal x854=((1.0)*pz);
2731 IkReal x855=(py*x850);
2732 IkReal x856=(j0*x851);
2733 evalcond[0]=((((-1.0)*x850*x854))+((j0*x850))+((py*x851)));
2734 evalcond[1]=(((x850*x852))+(((-0.9)*x850))+(((-1.0)*x850*x853))+py);
2735 evalcond[2]=((((-0.9)*x851))+pz+((x851*x852))+(((-1.0)*j0))+(((-1.0)*x851*x853)));
2736 evalcond[3]=((0.9)+(((-1.0)*x852))+(((-1.0)*x855))+x853+x856+(((-1.0)*x851*x854)));
2737 evalcond[4]=((-1.62)+(((-1.98)*cj3))+(((1.8)*x855))+(((-1.8)*x856))+(((0.099)*sj3))+(((1.8)*pz*x851)));
2744 rotationfunction0(solutions);
2755 IkReal j1array[1], cj1array[1], sj1array[1];
2756 bool j1valid[1]={
false};
2758 IkReal x857=((0.055)*sj3);
2759 IkReal x858=((1.1)*cj3);
2768 j1array[0]=((-1.5707963267949)+(x859.
value)+(((1.5707963267949)*(x860.
value))));
2769 sj1array[0]=
IKsin(j1array[0]);
2770 cj1array[0]=
IKcos(j1array[0]);
2771 if( j1array[0] >
IKPI )
2775 else if( j1array[0] < -
IKPI )
2776 { j1array[0]+=
IK2PI;
2779 for(
int ij1 = 0; ij1 < 1; ++ij1)
2785 _ij1[0] = ij1; _ij1[1] = -1;
2786 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2790 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2793 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2796 IkReal x861=
IKcos(j1);
2797 IkReal x862=
IKsin(j1);
2798 IkReal x863=((0.055)*sj3);
2799 IkReal x864=((1.1)*cj3);
2800 IkReal x865=((1.0)*pz);
2801 IkReal x866=(py*x861);
2802 IkReal x867=(j0*x862);
2803 evalcond[0]=((((-1.0)*x861*x865))+((py*x862))+((j0*x861)));
2804 evalcond[1]=((((-1.0)*x861*x864))+py+((x861*x863))+(((-0.9)*x861)));
2805 evalcond[2]=((((-1.0)*x862*x864))+pz+((x862*x863))+(((-1.0)*j0))+(((-0.9)*x862)));
2806 evalcond[3]=((0.9)+(((-1.0)*x863))+(((-1.0)*x866))+(((-1.0)*x862*x865))+x867+x864);
2807 evalcond[4]=((-1.62)+(((1.8)*pz*x862))+(((-1.98)*cj3))+(((1.8)*x866))+(((-1.8)*x867))+(((0.099)*sj3)));
2814 rotationfunction0(solutions);
2825 IkReal j1array[1], cj1array[1], sj1array[1];
2826 bool j1valid[1]={
false};
2836 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x868.
value)))+(x869.
value));
2837 sj1array[0]=
IKsin(j1array[0]);
2838 cj1array[0]=
IKcos(j1array[0]);
2839 if( j1array[0] >
IKPI )
2843 else if( j1array[0] < -
IKPI )
2844 { j1array[0]+=
IK2PI;
2847 for(
int ij1 = 0; ij1 < 1; ++ij1)
2853 _ij1[0] = ij1; _ij1[1] = -1;
2854 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2858 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2861 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2864 IkReal x870=
IKcos(j1);
2865 IkReal x871=
IKsin(j1);
2866 IkReal x872=((0.055)*sj3);
2867 IkReal x873=((1.1)*cj3);
2868 IkReal x874=((1.0)*pz);
2869 IkReal x875=(py*x870);
2870 IkReal x876=(j0*x871);
2871 evalcond[0]=((((-1.0)*x870*x874))+((py*x871))+((j0*x870)));
2872 evalcond[1]=(((x870*x872))+py+(((-1.0)*x870*x873))+(((-0.9)*x870)));
2873 evalcond[2]=(((x871*x872))+(((-1.0)*x871*x873))+pz+(((-1.0)*j0))+(((-0.9)*x871)));
2874 evalcond[3]=((0.9)+(((-1.0)*x871*x874))+(((-1.0)*x875))+x873+x876+(((-1.0)*x872)));
2875 evalcond[4]=((-1.62)+(((1.8)*pz*x871))+(((-1.98)*cj3))+(((1.8)*x875))+(((-1.8)*x876))+(((0.099)*sj3)));
2882 rotationfunction0(solutions);
2892 if( bgotonextstatement )
2894 bool bgotonextstatement =
true;
2899 bgotonextstatement=
false;
2904 if( bgotonextstatement )
2916 IkReal j1array[1], cj1array[1], sj1array[1];
2917 bool j1valid[1]={
false};
2919 IkReal x877=((1.1)*cj3);
2920 IkReal x878=((0.055)*sj3);
2921 IkReal x879=((0.35)*cj2);
2922 IkReal x880=(px*sj2);
2927 CheckValue<IkReal> x882 =
IKatan2WithCheck(IkReal((((pz*x880))+(((-1.0)*j0*x880))+(((-1.25)*pz))+(((-1.0)*pz*x879))+(((-1.0)*pz*x877))+((j0*x877))+((j0*x879))+(((-1.0)*j0*x878))+((pz*x878))+(((1.25)*j0)))),IkReal(((((-1.25)*py))+((py*x880))+(((-1.0)*py*x879))+(((-1.0)*py*x877))+((py*x878)))),
IKFAST_ATAN2_MAGTHRESH);
2931 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x881.
value)))+(x882.
value));
2932 sj1array[0]=
IKsin(j1array[0]);
2933 cj1array[0]=
IKcos(j1array[0]);
2934 if( j1array[0] >
IKPI )
2938 else if( j1array[0] < -
IKPI )
2939 { j1array[0]+=
IK2PI;
2942 for(
int ij1 = 0; ij1 < 1; ++ij1)
2948 _ij1[0] = ij1; _ij1[1] = -1;
2949 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
2953 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
2956 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
2959 IkReal x883=
IKcos(j1);
2960 IkReal x884=
IKsin(j1);
2961 IkReal x885=((0.055)*cj3);
2962 IkReal x886=((1.1)*cj3);
2963 IkReal x887=((0.055)*sj3);
2964 IkReal x888=((1.0)*pz);
2965 IkReal x889=(px*sj2);
2966 IkReal x890=((1.1)*sj3);
2967 IkReal x891=((1.0)*j0);
2968 IkReal x892=(sj2*x884);
2969 IkReal x893=(cj2*x883);
2970 IkReal x894=(pz*x884);
2971 IkReal x895=(sj2*x883);
2972 IkReal x896=(cj2*x884);
2973 evalcond[0]=((((-1.0)*x883*x888))+((py*x884))+((j0*x883)));
2974 evalcond[1]=((1.25)+((cj2*x894))+((py*x893))+(((-1.0)*x889))+(((-1.0)*x887))+x886+(((-1.0)*x891*x896))+(((0.35)*cj2)));
2975 evalcond[2]=((((-0.35)*sj2))+(((-1.0)*cj2*px))+(((-1.0)*x888*x892))+(((-1.0)*py*x895))+x885+x890+((j0*x892)));
2976 evalcond[3]=(((x886*x893))+(((1.25)*x893))+(((0.35)*x883))+py+(((-1.0)*x885*x895))+(((-1.0)*x890*x895))+(((-1.0)*x887*x893)));
2977 evalcond[4]=(((x886*x896))+(((1.25)*x896))+(((0.35)*x884))+(((-1.0)*x891))+pz+(((-1.0)*x885*x892))+(((-1.0)*x890*x892))+(((-1.0)*x887*x896)));
2978 evalcond[5]=((-0.471975)+(((2.5)*x889))+(((-2.5)*py*x893))+(((2.5)*j0*x896))+(((-1.0)*pp))+(((-0.7)*py*x883))+(((2.0)*j0*pz))+(((-0.7)*x894))+(((-1.0)*j0*x891))+(((-0.875)*cj2))+(((0.7)*j0*x884))+(((-2.5)*cj2*x894)));
2985 rotationfunction0(solutions);
2996 IkReal j1array[1], cj1array[1], sj1array[1];
2997 bool j1valid[1]={
false};
2999 IkReal x897=((1.1)*sj3);
3000 IkReal x898=(pz*sj2);
3001 IkReal x899=((1.0)*sj2);
3002 IkReal x900=(cj2*px);
3003 IkReal x901=((0.055)*cj3);
3004 IkReal x902=((0.35)*sj2);
3009 CheckValue<IkReal> x904 =
IKatan2WithCheck(IkReal(((((-1.0)*j0*x902))+(((0.35)*x898))+(((-1.0)*j0*x900))+(((-1.0)*pz*x901))+(((-1.0)*pz*x897))+((j0*x901))+((pz*x900))+((j0*x897)))),IkReal(((((-1.0)*py*x897))+(((-1.0)*py*x901))+((py*x902))+((py*x900)))),
IKFAST_ATAN2_MAGTHRESH);
3013 j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x903.
value)))+(x904.
value));
3014 sj1array[0]=
IKsin(j1array[0]);
3015 cj1array[0]=
IKcos(j1array[0]);
3016 if( j1array[0] >
IKPI )
3020 else if( j1array[0] < -
IKPI )
3021 { j1array[0]+=
IK2PI;
3024 for(
int ij1 = 0; ij1 < 1; ++ij1)
3030 _ij1[0] = ij1; _ij1[1] = -1;
3031 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3035 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3038 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3041 IkReal x905=
IKcos(j1);
3042 IkReal x906=
IKsin(j1);
3043 IkReal x907=((0.055)*cj3);
3044 IkReal x908=((1.1)*cj3);
3045 IkReal x909=((0.055)*sj3);
3046 IkReal x910=((1.0)*pz);
3047 IkReal x911=(px*sj2);
3048 IkReal x912=((1.1)*sj3);
3049 IkReal x913=((1.0)*j0);
3050 IkReal x914=(sj2*x906);
3051 IkReal x915=(cj2*x905);
3052 IkReal x916=(pz*x906);
3053 IkReal x917=(sj2*x905);
3054 IkReal x918=(cj2*x906);
3055 evalcond[0]=(((j0*x905))+(((-1.0)*x905*x910))+((py*x906)));
3056 evalcond[1]=((1.25)+(((-1.0)*x913*x918))+(((-1.0)*x909))+((cj2*x916))+(((-1.0)*x911))+x908+((py*x915))+(((0.35)*cj2)));
3057 evalcond[2]=(((j0*x914))+(((-0.35)*sj2))+(((-1.0)*cj2*px))+(((-1.0)*x910*x914))+(((-1.0)*py*x917))+x912+x907);
3058 evalcond[3]=((((0.35)*x905))+(((1.25)*x915))+py+(((-1.0)*x912*x917))+((x908*x915))+(((-1.0)*x907*x917))+(((-1.0)*x909*x915)));
3059 evalcond[4]=((((-1.0)*x913))+(((0.35)*x906))+(((1.25)*x918))+pz+(((-1.0)*x912*x914))+((x908*x918))+(((-1.0)*x907*x914))+(((-1.0)*x909*x918)));
3060 evalcond[5]=((-0.471975)+(((0.7)*j0*x906))+(((-1.0)*j0*x913))+(((-0.7)*x916))+(((-2.5)*py*x915))+(((-1.0)*pp))+(((-2.5)*cj2*x916))+(((2.0)*j0*pz))+(((2.5)*x911))+(((-0.875)*cj2))+(((2.5)*j0*x918))+(((-0.7)*py*x905)));
3067 rotationfunction0(solutions);
3078 IkReal j1array[1], cj1array[1], sj1array[1];
3079 bool j1valid[1]={
false};
3089 j1array[0]=((-1.5707963267949)+(x919.
value)+(((1.5707963267949)*(x920.
value))));
3090 sj1array[0]=
IKsin(j1array[0]);
3091 cj1array[0]=
IKcos(j1array[0]);
3092 if( j1array[0] >
IKPI )
3096 else if( j1array[0] < -
IKPI )
3097 { j1array[0]+=
IK2PI;
3100 for(
int ij1 = 0; ij1 < 1; ++ij1)
3106 _ij1[0] = ij1; _ij1[1] = -1;
3107 for(
int iij1 = ij1+1; iij1 < 1; ++iij1)
3111 j1valid[iij1]=
false; _ij1[1] = iij1;
break;
3114 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
3117 IkReal x921=
IKcos(j1);
3118 IkReal x922=
IKsin(j1);
3119 IkReal x923=((0.055)*cj3);
3120 IkReal x924=((1.1)*cj3);
3121 IkReal x925=((0.055)*sj3);
3122 IkReal x926=((1.0)*pz);
3123 IkReal x927=(px*sj2);
3124 IkReal x928=((1.1)*sj3);
3125 IkReal x929=((1.0)*j0);
3126 IkReal x930=(sj2*x922);
3127 IkReal x931=(cj2*x921);
3128 IkReal x932=(pz*x922);
3129 IkReal x933=(sj2*x921);
3130 IkReal x934=(cj2*x922);
3131 evalcond[0]=(((j0*x921))+((py*x922))+(((-1.0)*x921*x926)));
3132 evalcond[1]=((1.25)+(((-1.0)*x925))+((cj2*x932))+((py*x931))+(((-1.0)*x927))+(((-1.0)*x929*x934))+x924+(((0.35)*cj2)));
3133 evalcond[2]=(((j0*x930))+(((-0.35)*sj2))+(((-1.0)*cj2*px))+(((-1.0)*x926*x930))+(((-1.0)*py*x933))+x928+x923);
3134 evalcond[3]=((((-1.0)*x925*x931))+(((0.35)*x921))+((x924*x931))+(((-1.0)*x928*x933))+py+(((1.25)*x931))+(((-1.0)*x923*x933)));
3135 evalcond[4]=((((-1.0)*x929))+(((-1.0)*x925*x934))+(((0.35)*x922))+((x924*x934))+(((-1.0)*x928*x930))+pz+(((1.25)*x934))+(((-1.0)*x923*x930)));
3136 evalcond[5]=((-0.471975)+(((-2.5)*cj2*x932))+(((-0.7)*x932))+(((-1.0)*j0*x929))+(((-1.0)*pp))+(((2.0)*j0*pz))+(((-2.5)*py*x931))+(((-0.7)*py*x921))+(((2.5)*x927))+(((-0.875)*cj2))+(((0.7)*j0*x922))+(((2.5)*j0*x934)));
3143 rotationfunction0(solutions);
3162 for(
int rotationiter = 0; rotationiter < 1; ++rotationiter) {
3163 IkReal x73=((1.0)*r10);
3164 IkReal x74=((1.0)*sj1);
3165 IkReal x75=((1.0)*r11);
3166 IkReal x76=((1.0)*cj2);
3167 IkReal x77=((1.0)*r12);
3168 IkReal x78=(cj3*sj2);
3169 IkReal x79=(((cj2*sj3))+x78);
3170 IkReal x80=(((sj2*sj3))+(((-1.0)*cj3*x76)));
3171 IkReal x81=(cj1*x79);
3172 IkReal x82=(cj1*x80);
3173 IkReal x83=((((-1.0)*sj3*x76))+(((-1.0)*x78)));
3174 new_r00=((((-1.0)*x73*x81))+(((-1.0)*r20*x74*x79))+((r00*x80)));
3175 new_r01=((((-1.0)*r21*x74*x79))+((r01*x80))+(((-1.0)*x75*x81)));
3176 new_r02=((((-1.0)*x77*x81))+(((-1.0)*r22*x74*x79))+((r02*x80)));
3177 new_r10=((((-1.0)*sj1*x73))+((cj1*r20)));
3178 new_r11=(((cj1*r21))+(((-1.0)*r11*x74)));
3179 new_r12=(((cj1*r22))+(((-1.0)*r12*x74)));
3180 new_r20=((((-1.0)*x73*x82))+((r00*x83))+(((-1.0)*r20*x74*x80)));
3181 new_r21=((((-1.0)*r21*x74*x80))+((r01*x83))+(((-1.0)*x75*x82)));
3182 new_r22=((((-1.0)*r22*x74*x80))+(((-1.0)*x77*x82))+((r02*x83)));
3184 IkReal j5array[2], cj5array[2], sj5array[2];
3185 bool j5valid[2]={
false};
3187 cj5array[0]=new_r22;
3190 j5valid[0] = j5valid[1] =
true;
3191 j5array[0] =
IKacos(cj5array[0]);
3192 sj5array[0] =
IKsin(j5array[0]);
3193 cj5array[1] = cj5array[0];
3194 j5array[1] = -j5array[0];
3195 sj5array[1] = -sj5array[0];
3197 else if( isnan(cj5array[0]) )
3201 cj5array[0] = 1; sj5array[0] = 0; j5array[0] = 0;
3203 for(
int ij5 = 0; ij5 < 2; ++ij5)
3209 _ij5[0] = ij5; _ij5[1] = -1;
3210 for(
int iij5 = ij5+1; iij5 < 2; ++iij5)
3214 j5valid[iij5]=
false; _ij5[1] = iij5;
break;
3217 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3222 j4eval[1]=((
IKabs(new_r12))+(
IKabs(new_r02)));
3224 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 ||
IKabs(j4eval[2]) < 0.0000010000000000 )
3229 j6eval[1]=((
IKabs(new_r20))+(
IKabs(new_r21)));
3231 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
3237 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 )
3241 bool bgotonextstatement =
true;
3244 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
3245 evalcond[1]=new_r20;
3246 evalcond[2]=new_r02;
3247 evalcond[3]=new_r12;
3248 evalcond[4]=new_r21;
3249 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
3251 bgotonextstatement=
false;
3257 j4=
IKatan2(((-1.0)*new_r01), new_r00);
3259 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3260 vinfos[0].jointtype = 17;
3261 vinfos[0].foffset = j0;
3262 vinfos[0].indices[0] = _ij0[0];
3263 vinfos[0].indices[1] = _ij0[1];
3264 vinfos[0].maxsolutions = _nj0;
3265 vinfos[1].jointtype = 1;
3266 vinfos[1].foffset = j1;
3267 vinfos[1].indices[0] = _ij1[0];
3268 vinfos[1].indices[1] = _ij1[1];
3269 vinfos[1].maxsolutions = _nj1;
3270 vinfos[2].jointtype = 1;
3271 vinfos[2].foffset = j2;
3272 vinfos[2].indices[0] = _ij2[0];
3273 vinfos[2].indices[1] = _ij2[1];
3274 vinfos[2].maxsolutions = _nj2;
3275 vinfos[3].jointtype = 1;
3276 vinfos[3].foffset = j3;
3277 vinfos[3].indices[0] = _ij3[0];
3278 vinfos[3].indices[1] = _ij3[1];
3279 vinfos[3].maxsolutions = _nj3;
3280 vinfos[4].jointtype = 1;
3281 vinfos[4].foffset = j4;
3282 vinfos[4].fmul = j4mul;
3283 vinfos[4].freeind = 0;
3284 vinfos[4].maxsolutions = 0;
3285 vinfos[5].jointtype = 1;
3286 vinfos[5].foffset = j5;
3287 vinfos[5].indices[0] = _ij5[0];
3288 vinfos[5].indices[1] = _ij5[1];
3289 vinfos[5].maxsolutions = _nj5;
3290 vinfos[6].jointtype = 1;
3291 vinfos[6].foffset = j6;
3292 vinfos[6].fmul = j6mul;
3293 vinfos[6].freeind = 0;
3294 vinfos[6].maxsolutions = 0;
3295 std::vector<int> vfree(1);
3302 if( bgotonextstatement )
3304 bool bgotonextstatement =
true;
3307 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
3308 evalcond[1]=new_r20;
3309 evalcond[2]=new_r02;
3310 evalcond[3]=new_r12;
3311 evalcond[4]=new_r21;
3312 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
3314 bgotonextstatement=
false;
3320 j4=
IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
3322 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3323 vinfos[0].jointtype = 17;
3324 vinfos[0].foffset = j0;
3325 vinfos[0].indices[0] = _ij0[0];
3326 vinfos[0].indices[1] = _ij0[1];
3327 vinfos[0].maxsolutions = _nj0;
3328 vinfos[1].jointtype = 1;
3329 vinfos[1].foffset = j1;
3330 vinfos[1].indices[0] = _ij1[0];
3331 vinfos[1].indices[1] = _ij1[1];
3332 vinfos[1].maxsolutions = _nj1;
3333 vinfos[2].jointtype = 1;
3334 vinfos[2].foffset = j2;
3335 vinfos[2].indices[0] = _ij2[0];
3336 vinfos[2].indices[1] = _ij2[1];
3337 vinfos[2].maxsolutions = _nj2;
3338 vinfos[3].jointtype = 1;
3339 vinfos[3].foffset = j3;
3340 vinfos[3].indices[0] = _ij3[0];
3341 vinfos[3].indices[1] = _ij3[1];
3342 vinfos[3].maxsolutions = _nj3;
3343 vinfos[4].jointtype = 1;
3344 vinfos[4].foffset = j4;
3345 vinfos[4].fmul = j4mul;
3346 vinfos[4].freeind = 0;
3347 vinfos[4].maxsolutions = 0;
3348 vinfos[5].jointtype = 1;
3349 vinfos[5].foffset = j5;
3350 vinfos[5].indices[0] = _ij5[0];
3351 vinfos[5].indices[1] = _ij5[1];
3352 vinfos[5].maxsolutions = _nj5;
3353 vinfos[6].jointtype = 1;
3354 vinfos[6].foffset = j6;
3355 vinfos[6].fmul = j6mul;
3356 vinfos[6].freeind = 0;
3357 vinfos[6].maxsolutions = 0;
3358 std::vector<int> vfree(1);
3365 if( bgotonextstatement )
3367 bool bgotonextstatement =
true;
3370 evalcond[0]=((
IKabs(new_r12))+(
IKabs(new_r02)));
3371 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3373 bgotonextstatement=
false;
3380 IkReal x84=new_r22*new_r22;
3381 IkReal x85=((16.0)*new_r10);
3382 IkReal x86=((16.0)*new_r01);
3383 IkReal x87=((16.0)*new_r22);
3384 IkReal x88=((8.0)*new_r11);
3385 IkReal x89=((8.0)*new_r00);
3386 IkReal x90=(x84*x85);
3387 IkReal x91=(x84*x86);
3388 j4eval[0]=((
IKabs((x86+(((-1.0)*x91)))))+(
IKabs((x91+(((-1.0)*x86)))))+(
IKabs((((x84*x88))+(((-1.0)*new_r22*x89)))))+(
IKabs((x85+(((-1.0)*x90)))))+(
IKabs((((new_r22*x88))+(((-1.0)*x89)))))+(
IKabs(((((32.0)*new_r11))+(((-16.0)*new_r11*x84))+(((-1.0)*new_r00*x87)))))+(
IKabs(((((16.0)*new_r00))+((new_r11*x87))+(((-32.0)*new_r00*x84)))))+(
IKabs((x90+(((-1.0)*x85))))));
3389 if(
IKabs(j4eval[0]) < 0.0000000100000000 )
3395 IkReal op[4+1], zeror[4];
3397 IkReal j4evalpoly[1];
3398 IkReal x92=new_r22*new_r22;
3399 IkReal x93=((16.0)*new_r10);
3400 IkReal x94=(new_r11*new_r22);
3401 IkReal x95=(x92*x93);
3402 IkReal x96=((((8.0)*x94))+(((-8.0)*new_r00)));
3404 op[1]=(x93+(((-1.0)*x95)));
3405 op[2]=((((16.0)*x94))+(((16.0)*new_r00))+(((-32.0)*new_r00*x92)));
3406 op[3]=(x95+(((-1.0)*x93)));
3408 polyroots4(op,zeror,numroots);
3409 IkReal j4array[4], cj4array[4], sj4array[4], tempj4array[1];
3410 int numsolutions = 0;
3411 for(
int ij4 = 0; ij4 < numroots; ++ij4)
3413 IkReal htj4 = zeror[ij4];
3414 tempj4array[0]=((2.0)*(
atan(htj4)));
3415 for(
int kj4 = 0; kj4 < 1; ++kj4)
3417 j4array[numsolutions] = tempj4array[kj4];
3418 if( j4array[numsolutions] >
IKPI )
3420 j4array[numsolutions]-=
IK2PI;
3422 else if( j4array[numsolutions] < -
IKPI )
3424 j4array[numsolutions]+=
IK2PI;
3426 sj4array[numsolutions] =
IKsin(j4array[numsolutions]);
3427 cj4array[numsolutions] =
IKcos(j4array[numsolutions]);
3431 bool j4valid[4]={
true,
true,
true,
true};
3433 for(
int ij4 = 0; ij4 < numsolutions; ++ij4)
3439 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
3442 IkReal x97=new_r22*new_r22;
3443 IkReal x98=((16.0)*new_r01);
3444 IkReal x99=(new_r00*new_r22);
3445 IkReal x100=((8.0)*x99);
3446 IkReal x101=(new_r11*x97);
3447 IkReal x102=(x97*x98);
3448 IkReal x103=((8.0)*x101);
3449 j4evalpoly[0]=((((htj4*htj4)*(((((32.0)*new_r11))+(((-16.0)*x99))+(((-16.0)*x101))))))+(((-1.0)*x100))+x103+(((htj4*htj4*htj4*htj4)*(((((-1.0)*x100))+x103))))+((htj4*(((((-1.0)*x102))+x98))))+(((htj4*htj4*htj4)*((x102+(((-1.0)*x98)))))));
3450 if(
IKabs(j4evalpoly[0]) > 0.0000001000000000 )
3454 _ij4[0] = ij4; _ij4[1] = -1;
3455 for(
int iij4 = ij4+1; iij4 < numsolutions; ++iij4)
3459 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
3468 IkReal x104=cj4*cj4;
3469 IkReal x105=(cj4*new_r22);
3470 IkReal x106=((-1.0)+x104+(((-1.0)*x104*(new_r22*new_r22))));
3472 j6eval[1]=((
IKabs((((new_r00*sj4))+((new_r01*x105)))))+(
IKabs((((new_r01*sj4))+(((-1.0)*new_r00*x105))))));
3474 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
3483 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
3491 IkReal x107=new_r22*new_r22;
3492 j6eval[0]=(((cj4*x107))+(((-1.0)*cj4)));
3493 j6eval[1]=((((-1.0)*sj4))+((sj4*x107)));
3494 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
3498 bool bgotonextstatement =
true;
3501 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
3502 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3504 bgotonextstatement=
false;
3506 IkReal j6array[1], cj6array[1], sj6array[1];
3507 bool j6valid[1]={
false};
3511 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3512 sj6array[0]=
IKsin(j6array[0]);
3513 cj6array[0]=
IKcos(j6array[0]);
3514 if( j6array[0] >
IKPI )
3518 else if( j6array[0] < -
IKPI )
3519 { j6array[0]+=
IK2PI;
3522 for(
int ij6 = 0; ij6 < 1; ++ij6)
3528 _ij6[0] = ij6; _ij6[1] = -1;
3529 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
3533 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
3536 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3539 IkReal x108=
IKsin(j6);
3540 IkReal x109=
IKcos(j6);
3542 evalcond[1]=((-1.0)*x108);
3543 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x108)));
3544 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x109)));
3552 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3553 vinfos[0].jointtype = 17;
3554 vinfos[0].foffset = j0;
3555 vinfos[0].indices[0] = _ij0[0];
3556 vinfos[0].indices[1] = _ij0[1];
3557 vinfos[0].maxsolutions = _nj0;
3558 vinfos[1].jointtype = 1;
3559 vinfos[1].foffset = j1;
3560 vinfos[1].indices[0] = _ij1[0];
3561 vinfos[1].indices[1] = _ij1[1];
3562 vinfos[1].maxsolutions = _nj1;
3563 vinfos[2].jointtype = 1;
3564 vinfos[2].foffset = j2;
3565 vinfos[2].indices[0] = _ij2[0];
3566 vinfos[2].indices[1] = _ij2[1];
3567 vinfos[2].maxsolutions = _nj2;
3568 vinfos[3].jointtype = 1;
3569 vinfos[3].foffset = j3;
3570 vinfos[3].indices[0] = _ij3[0];
3571 vinfos[3].indices[1] = _ij3[1];
3572 vinfos[3].maxsolutions = _nj3;
3573 vinfos[4].jointtype = 1;
3574 vinfos[4].foffset = j4;
3575 vinfos[4].indices[0] = _ij4[0];
3576 vinfos[4].indices[1] = _ij4[1];
3577 vinfos[4].maxsolutions = _nj4;
3578 vinfos[5].jointtype = 1;
3579 vinfos[5].foffset = j5;
3580 vinfos[5].indices[0] = _ij5[0];
3581 vinfos[5].indices[1] = _ij5[1];
3582 vinfos[5].maxsolutions = _nj5;
3583 vinfos[6].jointtype = 1;
3584 vinfos[6].foffset = j6;
3585 vinfos[6].indices[0] = _ij6[0];
3586 vinfos[6].indices[1] = _ij6[1];
3587 vinfos[6].maxsolutions = _nj6;
3588 std::vector<int> vfree(0);
3596 if( bgotonextstatement )
3598 bool bgotonextstatement =
true;
3601 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
3602 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3604 bgotonextstatement=
false;
3606 IkReal j6array[1], cj6array[1], sj6array[1];
3607 bool j6valid[1]={
false};
3611 j6array[0]=
IKatan2(new_r00, new_r01);
3612 sj6array[0]=
IKsin(j6array[0]);
3613 cj6array[0]=
IKcos(j6array[0]);
3614 if( j6array[0] >
IKPI )
3618 else if( j6array[0] < -
IKPI )
3619 { j6array[0]+=
IK2PI;
3622 for(
int ij6 = 0; ij6 < 1; ++ij6)
3628 _ij6[0] = ij6; _ij6[1] = -1;
3629 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
3633 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
3636 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3639 IkReal x110=
IKsin(j6);
3640 IkReal x111=
IKcos(j6);
3642 evalcond[1]=((-1.0)*x110);
3643 evalcond[2]=(new_r00+(((-1.0)*x110)));
3644 evalcond[3]=(new_r01+(((-1.0)*x111)));
3652 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3653 vinfos[0].jointtype = 17;
3654 vinfos[0].foffset = j0;
3655 vinfos[0].indices[0] = _ij0[0];
3656 vinfos[0].indices[1] = _ij0[1];
3657 vinfos[0].maxsolutions = _nj0;
3658 vinfos[1].jointtype = 1;
3659 vinfos[1].foffset = j1;
3660 vinfos[1].indices[0] = _ij1[0];
3661 vinfos[1].indices[1] = _ij1[1];
3662 vinfos[1].maxsolutions = _nj1;
3663 vinfos[2].jointtype = 1;
3664 vinfos[2].foffset = j2;
3665 vinfos[2].indices[0] = _ij2[0];
3666 vinfos[2].indices[1] = _ij2[1];
3667 vinfos[2].maxsolutions = _nj2;
3668 vinfos[3].jointtype = 1;
3669 vinfos[3].foffset = j3;
3670 vinfos[3].indices[0] = _ij3[0];
3671 vinfos[3].indices[1] = _ij3[1];
3672 vinfos[3].maxsolutions = _nj3;
3673 vinfos[4].jointtype = 1;
3674 vinfos[4].foffset = j4;
3675 vinfos[4].indices[0] = _ij4[0];
3676 vinfos[4].indices[1] = _ij4[1];
3677 vinfos[4].maxsolutions = _nj4;
3678 vinfos[5].jointtype = 1;
3679 vinfos[5].foffset = j5;
3680 vinfos[5].indices[0] = _ij5[0];
3681 vinfos[5].indices[1] = _ij5[1];
3682 vinfos[5].maxsolutions = _nj5;
3683 vinfos[6].jointtype = 1;
3684 vinfos[6].foffset = j6;
3685 vinfos[6].indices[0] = _ij6[0];
3686 vinfos[6].indices[1] = _ij6[1];
3687 vinfos[6].maxsolutions = _nj6;
3688 std::vector<int> vfree(0);
3696 if( bgotonextstatement )
3698 bool bgotonextstatement =
true;
3701 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
3702 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3704 bgotonextstatement=
false;
3706 IkReal j6array[1], cj6array[1], sj6array[1];
3707 bool j6valid[1]={
false};
3711 j6array[0]=
IKatan2(new_r10, new_r11);
3712 sj6array[0]=
IKsin(j6array[0]);
3713 cj6array[0]=
IKcos(j6array[0]);
3714 if( j6array[0] >
IKPI )
3718 else if( j6array[0] < -
IKPI )
3719 { j6array[0]+=
IK2PI;
3722 for(
int ij6 = 0; ij6 < 1; ++ij6)
3728 _ij6[0] = ij6; _ij6[1] = -1;
3729 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
3733 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
3736 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3739 IkReal x112=
IKsin(j6);
3740 IkReal x113=
IKcos(j6);
3742 evalcond[1]=((-1.0)*x112);
3743 evalcond[2]=(new_r10+(((-1.0)*x112)));
3744 evalcond[3]=(new_r11+(((-1.0)*x113)));
3752 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3753 vinfos[0].jointtype = 17;
3754 vinfos[0].foffset = j0;
3755 vinfos[0].indices[0] = _ij0[0];
3756 vinfos[0].indices[1] = _ij0[1];
3757 vinfos[0].maxsolutions = _nj0;
3758 vinfos[1].jointtype = 1;
3759 vinfos[1].foffset = j1;
3760 vinfos[1].indices[0] = _ij1[0];
3761 vinfos[1].indices[1] = _ij1[1];
3762 vinfos[1].maxsolutions = _nj1;
3763 vinfos[2].jointtype = 1;
3764 vinfos[2].foffset = j2;
3765 vinfos[2].indices[0] = _ij2[0];
3766 vinfos[2].indices[1] = _ij2[1];
3767 vinfos[2].maxsolutions = _nj2;
3768 vinfos[3].jointtype = 1;
3769 vinfos[3].foffset = j3;
3770 vinfos[3].indices[0] = _ij3[0];
3771 vinfos[3].indices[1] = _ij3[1];
3772 vinfos[3].maxsolutions = _nj3;
3773 vinfos[4].jointtype = 1;
3774 vinfos[4].foffset = j4;
3775 vinfos[4].indices[0] = _ij4[0];
3776 vinfos[4].indices[1] = _ij4[1];
3777 vinfos[4].maxsolutions = _nj4;
3778 vinfos[5].jointtype = 1;
3779 vinfos[5].foffset = j5;
3780 vinfos[5].indices[0] = _ij5[0];
3781 vinfos[5].indices[1] = _ij5[1];
3782 vinfos[5].maxsolutions = _nj5;
3783 vinfos[6].jointtype = 1;
3784 vinfos[6].foffset = j6;
3785 vinfos[6].indices[0] = _ij6[0];
3786 vinfos[6].indices[1] = _ij6[1];
3787 vinfos[6].maxsolutions = _nj6;
3788 std::vector<int> vfree(0);
3796 if( bgotonextstatement )
3798 bool bgotonextstatement =
true;
3801 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
3802 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3804 bgotonextstatement=
false;
3806 IkReal j6array[1], cj6array[1], sj6array[1];
3807 bool j6valid[1]={
false};
3811 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3812 sj6array[0]=
IKsin(j6array[0]);
3813 cj6array[0]=
IKcos(j6array[0]);
3814 if( j6array[0] >
IKPI )
3818 else if( j6array[0] < -
IKPI )
3819 { j6array[0]+=
IK2PI;
3822 for(
int ij6 = 0; ij6 < 1; ++ij6)
3828 _ij6[0] = ij6; _ij6[1] = -1;
3829 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
3833 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
3836 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3839 IkReal x114=
IKsin(j6);
3840 IkReal x115=
IKcos(j6);
3842 evalcond[1]=((-1.0)*x114);
3843 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x114)));
3844 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x115)));
3852 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3853 vinfos[0].jointtype = 17;
3854 vinfos[0].foffset = j0;
3855 vinfos[0].indices[0] = _ij0[0];
3856 vinfos[0].indices[1] = _ij0[1];
3857 vinfos[0].maxsolutions = _nj0;
3858 vinfos[1].jointtype = 1;
3859 vinfos[1].foffset = j1;
3860 vinfos[1].indices[0] = _ij1[0];
3861 vinfos[1].indices[1] = _ij1[1];
3862 vinfos[1].maxsolutions = _nj1;
3863 vinfos[2].jointtype = 1;
3864 vinfos[2].foffset = j2;
3865 vinfos[2].indices[0] = _ij2[0];
3866 vinfos[2].indices[1] = _ij2[1];
3867 vinfos[2].maxsolutions = _nj2;
3868 vinfos[3].jointtype = 1;
3869 vinfos[3].foffset = j3;
3870 vinfos[3].indices[0] = _ij3[0];
3871 vinfos[3].indices[1] = _ij3[1];
3872 vinfos[3].maxsolutions = _nj3;
3873 vinfos[4].jointtype = 1;
3874 vinfos[4].foffset = j4;
3875 vinfos[4].indices[0] = _ij4[0];
3876 vinfos[4].indices[1] = _ij4[1];
3877 vinfos[4].maxsolutions = _nj4;
3878 vinfos[5].jointtype = 1;
3879 vinfos[5].foffset = j5;
3880 vinfos[5].indices[0] = _ij5[0];
3881 vinfos[5].indices[1] = _ij5[1];
3882 vinfos[5].maxsolutions = _nj5;
3883 vinfos[6].jointtype = 1;
3884 vinfos[6].foffset = j6;
3885 vinfos[6].indices[0] = _ij6[0];
3886 vinfos[6].indices[1] = _ij6[1];
3887 vinfos[6].maxsolutions = _nj6;
3888 std::vector<int> vfree(0);
3896 if( bgotonextstatement )
3898 bool bgotonextstatement =
true;
3905 if((x116.
value) < -0.00001)
3908 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.0)+(
IKsign(sj4)))))+(
IKabs((cj4+(((-1.0)*gconst24)))))), 6.28318530717959)));
3909 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
3911 bgotonextstatement=
false;
3918 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
3920 sj4=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
3929 if((x117.
value) < -0.00001)
3932 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
3933 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
3936 IkReal j6array[1], cj6array[1], sj6array[1];
3937 bool j6valid[1]={
false};
3939 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
3947 j6array[0]=
IKatan2(((((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x118.
value)));
3948 sj6array[0]=
IKsin(j6array[0]);
3949 cj6array[0]=
IKcos(j6array[0]);
3950 if( j6array[0] >
IKPI )
3954 else if( j6array[0] < -
IKPI )
3955 { j6array[0]+=
IK2PI;
3958 for(
int ij6 = 0; ij6 < 1; ++ij6)
3964 _ij6[0] = ij6; _ij6[1] = -1;
3965 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
3969 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
3972 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
3975 IkReal x119=
IKcos(j6);
3976 IkReal x120=
IKsin(j6);
3977 IkReal x121=((1.0)*x119);
3978 IkReal x122=((1.0)*x120);
3979 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
3981 IkReal x123=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
3982 IkReal x124=((1.0)*x123);
3984 evalcond[1]=((-1.0)*x120);
3985 evalcond[2]=((((-1.0)*gconst24*x121))+new_r11);
3986 evalcond[3]=((((-1.0)*gconst24*x122))+new_r10);
3987 evalcond[4]=(new_r01+((x119*x123)));
3988 evalcond[5]=(((x120*x123))+new_r00);
3989 evalcond[6]=((((-1.0)*new_r00*x124))+(((-1.0)*x122))+((gconst24*new_r10)));
3990 evalcond[7]=((((-1.0)*new_r01*x124))+(((-1.0)*x121))+((gconst24*new_r11)));
3998 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3999 vinfos[0].jointtype = 17;
4000 vinfos[0].foffset = j0;
4001 vinfos[0].indices[0] = _ij0[0];
4002 vinfos[0].indices[1] = _ij0[1];
4003 vinfos[0].maxsolutions = _nj0;
4004 vinfos[1].jointtype = 1;
4005 vinfos[1].foffset = j1;
4006 vinfos[1].indices[0] = _ij1[0];
4007 vinfos[1].indices[1] = _ij1[1];
4008 vinfos[1].maxsolutions = _nj1;
4009 vinfos[2].jointtype = 1;
4010 vinfos[2].foffset = j2;
4011 vinfos[2].indices[0] = _ij2[0];
4012 vinfos[2].indices[1] = _ij2[1];
4013 vinfos[2].maxsolutions = _nj2;
4014 vinfos[3].jointtype = 1;
4015 vinfos[3].foffset = j3;
4016 vinfos[3].indices[0] = _ij3[0];
4017 vinfos[3].indices[1] = _ij3[1];
4018 vinfos[3].maxsolutions = _nj3;
4019 vinfos[4].jointtype = 1;
4020 vinfos[4].foffset = j4;
4021 vinfos[4].indices[0] = _ij4[0];
4022 vinfos[4].indices[1] = _ij4[1];
4023 vinfos[4].maxsolutions = _nj4;
4024 vinfos[5].jointtype = 1;
4025 vinfos[5].foffset = j5;
4026 vinfos[5].indices[0] = _ij5[0];
4027 vinfos[5].indices[1] = _ij5[1];
4028 vinfos[5].maxsolutions = _nj5;
4029 vinfos[6].jointtype = 1;
4030 vinfos[6].foffset = j6;
4031 vinfos[6].indices[0] = _ij6[0];
4032 vinfos[6].indices[1] = _ij6[1];
4033 vinfos[6].maxsolutions = _nj6;
4034 std::vector<int> vfree(0);
4043 IkReal j6array[1], cj6array[1], sj6array[1];
4044 bool j6valid[1]={
false};
4054 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x125.
value)))+(x126.
value));
4055 sj6array[0]=
IKsin(j6array[0]);
4056 cj6array[0]=
IKcos(j6array[0]);
4057 if( j6array[0] >
IKPI )
4061 else if( j6array[0] < -
IKPI )
4062 { j6array[0]+=
IK2PI;
4065 for(
int ij6 = 0; ij6 < 1; ++ij6)
4071 _ij6[0] = ij6; _ij6[1] = -1;
4072 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4076 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4079 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4082 IkReal x127=
IKcos(j6);
4083 IkReal x128=
IKsin(j6);
4084 IkReal x129=((1.0)*x127);
4085 IkReal x130=((1.0)*x128);
4086 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
4088 IkReal x131=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
4089 IkReal x132=((1.0)*x131);
4091 evalcond[1]=((-1.0)*x128);
4092 evalcond[2]=((((-1.0)*gconst24*x129))+new_r11);
4093 evalcond[3]=((((-1.0)*gconst24*x130))+new_r10);
4094 evalcond[4]=(((x127*x131))+new_r01);
4095 evalcond[5]=(((x128*x131))+new_r00);
4096 evalcond[6]=((((-1.0)*new_r00*x132))+(((-1.0)*x130))+((gconst24*new_r10)));
4097 evalcond[7]=((((-1.0)*new_r01*x132))+(((-1.0)*x129))+((gconst24*new_r11)));
4105 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4106 vinfos[0].jointtype = 17;
4107 vinfos[0].foffset = j0;
4108 vinfos[0].indices[0] = _ij0[0];
4109 vinfos[0].indices[1] = _ij0[1];
4110 vinfos[0].maxsolutions = _nj0;
4111 vinfos[1].jointtype = 1;
4112 vinfos[1].foffset = j1;
4113 vinfos[1].indices[0] = _ij1[0];
4114 vinfos[1].indices[1] = _ij1[1];
4115 vinfos[1].maxsolutions = _nj1;
4116 vinfos[2].jointtype = 1;
4117 vinfos[2].foffset = j2;
4118 vinfos[2].indices[0] = _ij2[0];
4119 vinfos[2].indices[1] = _ij2[1];
4120 vinfos[2].maxsolutions = _nj2;
4121 vinfos[3].jointtype = 1;
4122 vinfos[3].foffset = j3;
4123 vinfos[3].indices[0] = _ij3[0];
4124 vinfos[3].indices[1] = _ij3[1];
4125 vinfos[3].maxsolutions = _nj3;
4126 vinfos[4].jointtype = 1;
4127 vinfos[4].foffset = j4;
4128 vinfos[4].indices[0] = _ij4[0];
4129 vinfos[4].indices[1] = _ij4[1];
4130 vinfos[4].maxsolutions = _nj4;
4131 vinfos[5].jointtype = 1;
4132 vinfos[5].foffset = j5;
4133 vinfos[5].indices[0] = _ij5[0];
4134 vinfos[5].indices[1] = _ij5[1];
4135 vinfos[5].maxsolutions = _nj5;
4136 vinfos[6].jointtype = 1;
4137 vinfos[6].foffset = j6;
4138 vinfos[6].indices[0] = _ij6[0];
4139 vinfos[6].indices[1] = _ij6[1];
4140 vinfos[6].maxsolutions = _nj6;
4141 std::vector<int> vfree(0);
4153 if( bgotonextstatement )
4155 bool bgotonextstatement =
true;
4162 if((x133.
value) < -0.00001)
4165 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj4+(((-1.0)*gconst24)))))+(
IKabs(((1.0)+(
IKsign(sj4)))))), 6.28318530717959)));
4166 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4168 bgotonextstatement=
false;
4175 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
4177 sj4=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))))));
4181 j4=((-1.0)*(
IKacos(gconst24)));
4186 if((x134.
value) < -0.00001)
4189 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
4190 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
4193 IkReal j6array[1], cj6array[1], sj6array[1];
4194 bool j6valid[1]={
false};
4196 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
4204 j6array[0]=
IKatan2((((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24))))))))+((gconst24*new_r10))), (new_r11*(x135.
value)));
4205 sj6array[0]=
IKsin(j6array[0]);
4206 cj6array[0]=
IKcos(j6array[0]);
4207 if( j6array[0] >
IKPI )
4211 else if( j6array[0] < -
IKPI )
4212 { j6array[0]+=
IK2PI;
4215 for(
int ij6 = 0; ij6 < 1; ++ij6)
4221 _ij6[0] = ij6; _ij6[1] = -1;
4222 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4226 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4229 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4232 IkReal x136=
IKcos(j6);
4233 IkReal x137=
IKsin(j6);
4234 IkReal x138=((1.0)*x136);
4235 IkReal x139=((1.0)*x137);
4236 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
4238 IkReal x140=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
4240 evalcond[1]=((-1.0)*x137);
4241 evalcond[2]=((((-1.0)*gconst24*x138))+new_r11);
4242 evalcond[3]=((((-1.0)*gconst24*x139))+new_r10);
4243 evalcond[4]=((((-1.0)*x138*x140))+new_r01);
4244 evalcond[5]=((((-1.0)*x139*x140))+new_r00);
4245 evalcond[6]=(((new_r00*x140))+(((-1.0)*x139))+((gconst24*new_r10)));
4246 evalcond[7]=(((new_r01*x140))+(((-1.0)*x138))+((gconst24*new_r11)));
4254 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4255 vinfos[0].jointtype = 17;
4256 vinfos[0].foffset = j0;
4257 vinfos[0].indices[0] = _ij0[0];
4258 vinfos[0].indices[1] = _ij0[1];
4259 vinfos[0].maxsolutions = _nj0;
4260 vinfos[1].jointtype = 1;
4261 vinfos[1].foffset = j1;
4262 vinfos[1].indices[0] = _ij1[0];
4263 vinfos[1].indices[1] = _ij1[1];
4264 vinfos[1].maxsolutions = _nj1;
4265 vinfos[2].jointtype = 1;
4266 vinfos[2].foffset = j2;
4267 vinfos[2].indices[0] = _ij2[0];
4268 vinfos[2].indices[1] = _ij2[1];
4269 vinfos[2].maxsolutions = _nj2;
4270 vinfos[3].jointtype = 1;
4271 vinfos[3].foffset = j3;
4272 vinfos[3].indices[0] = _ij3[0];
4273 vinfos[3].indices[1] = _ij3[1];
4274 vinfos[3].maxsolutions = _nj3;
4275 vinfos[4].jointtype = 1;
4276 vinfos[4].foffset = j4;
4277 vinfos[4].indices[0] = _ij4[0];
4278 vinfos[4].indices[1] = _ij4[1];
4279 vinfos[4].maxsolutions = _nj4;
4280 vinfos[5].jointtype = 1;
4281 vinfos[5].foffset = j5;
4282 vinfos[5].indices[0] = _ij5[0];
4283 vinfos[5].indices[1] = _ij5[1];
4284 vinfos[5].maxsolutions = _nj5;
4285 vinfos[6].jointtype = 1;
4286 vinfos[6].foffset = j6;
4287 vinfos[6].indices[0] = _ij6[0];
4288 vinfos[6].indices[1] = _ij6[1];
4289 vinfos[6].maxsolutions = _nj6;
4290 std::vector<int> vfree(0);
4299 IkReal j6array[1], cj6array[1], sj6array[1];
4300 bool j6valid[1]={
false};
4310 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x141.
value)))+(x142.
value));
4311 sj6array[0]=
IKsin(j6array[0]);
4312 cj6array[0]=
IKcos(j6array[0]);
4313 if( j6array[0] >
IKPI )
4317 else if( j6array[0] < -
IKPI )
4318 { j6array[0]+=
IK2PI;
4321 for(
int ij6 = 0; ij6 < 1; ++ij6)
4327 _ij6[0] = ij6; _ij6[1] = -1;
4328 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4332 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4335 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4338 IkReal x143=
IKcos(j6);
4339 IkReal x144=
IKsin(j6);
4340 IkReal x145=((1.0)*x143);
4341 IkReal x146=((1.0)*x144);
4342 if((((1.0)+(((-1.0)*(gconst24*gconst24))))) < -0.00001)
4344 IkReal x147=
IKsqrt(((1.0)+(((-1.0)*(gconst24*gconst24)))));
4346 evalcond[1]=((-1.0)*x144);
4347 evalcond[2]=((((-1.0)*gconst24*x145))+new_r11);
4348 evalcond[3]=((((-1.0)*gconst24*x146))+new_r10);
4349 evalcond[4]=((((-1.0)*x145*x147))+new_r01);
4350 evalcond[5]=((((-1.0)*x146*x147))+new_r00);
4351 evalcond[6]=((((-1.0)*x146))+((new_r00*x147))+((gconst24*new_r10)));
4352 evalcond[7]=((((-1.0)*x145))+((new_r01*x147))+((gconst24*new_r11)));
4360 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4361 vinfos[0].jointtype = 17;
4362 vinfos[0].foffset = j0;
4363 vinfos[0].indices[0] = _ij0[0];
4364 vinfos[0].indices[1] = _ij0[1];
4365 vinfos[0].maxsolutions = _nj0;
4366 vinfos[1].jointtype = 1;
4367 vinfos[1].foffset = j1;
4368 vinfos[1].indices[0] = _ij1[0];
4369 vinfos[1].indices[1] = _ij1[1];
4370 vinfos[1].maxsolutions = _nj1;
4371 vinfos[2].jointtype = 1;
4372 vinfos[2].foffset = j2;
4373 vinfos[2].indices[0] = _ij2[0];
4374 vinfos[2].indices[1] = _ij2[1];
4375 vinfos[2].maxsolutions = _nj2;
4376 vinfos[3].jointtype = 1;
4377 vinfos[3].foffset = j3;
4378 vinfos[3].indices[0] = _ij3[0];
4379 vinfos[3].indices[1] = _ij3[1];
4380 vinfos[3].maxsolutions = _nj3;
4381 vinfos[4].jointtype = 1;
4382 vinfos[4].foffset = j4;
4383 vinfos[4].indices[0] = _ij4[0];
4384 vinfos[4].indices[1] = _ij4[1];
4385 vinfos[4].maxsolutions = _nj4;
4386 vinfos[5].jointtype = 1;
4387 vinfos[5].foffset = j5;
4388 vinfos[5].indices[0] = _ij5[0];
4389 vinfos[5].indices[1] = _ij5[1];
4390 vinfos[5].maxsolutions = _nj5;
4391 vinfos[6].jointtype = 1;
4392 vinfos[6].foffset = j6;
4393 vinfos[6].indices[0] = _ij6[0];
4394 vinfos[6].indices[1] = _ij6[1];
4395 vinfos[6].maxsolutions = _nj6;
4396 std::vector<int> vfree(0);
4408 if( bgotonextstatement )
4410 bool bgotonextstatement =
true;
4417 if((x148.
value) < -0.00001)
4420 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.0)+(
IKsign(sj4)))))+(
IKabs((cj4+(((-1.0)*gconst25)))))), 6.28318530717959)));
4421 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4423 bgotonextstatement=
false;
4430 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
4432 sj4=
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))));
4441 if((x149.
value) < -0.00001)
4444 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
4445 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
4448 IkReal j6array[1], cj6array[1], sj6array[1];
4449 bool j6valid[1]={
false};
4451 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
4459 j6array[0]=
IKatan2((((gconst25*new_r10))+(((-1.0)*new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x150.
value)));
4460 sj6array[0]=
IKsin(j6array[0]);
4461 cj6array[0]=
IKcos(j6array[0]);
4462 if( j6array[0] >
IKPI )
4466 else if( j6array[0] < -
IKPI )
4467 { j6array[0]+=
IK2PI;
4470 for(
int ij6 = 0; ij6 < 1; ++ij6)
4476 _ij6[0] = ij6; _ij6[1] = -1;
4477 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4481 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4484 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4487 IkReal x151=
IKcos(j6);
4488 IkReal x152=
IKsin(j6);
4489 IkReal x153=((1.0)*gconst25);
4490 if((((1.0)+(((-1.0)*gconst25*x153)))) < -0.00001)
4492 IkReal x154=
IKsqrt(((1.0)+(((-1.0)*gconst25*x153))));
4493 IkReal x155=((1.0)*x154);
4495 evalcond[1]=((-1.0)*x152);
4496 evalcond[2]=((((-1.0)*x151*x153))+new_r11);
4497 evalcond[3]=((((-1.0)*x152*x153))+new_r10);
4498 evalcond[4]=(((x151*x154))+new_r01);
4499 evalcond[5]=(((x152*x154))+new_r00);
4500 evalcond[6]=(((gconst25*new_r10))+(((-1.0)*x152))+(((-1.0)*new_r00*x155)));
4501 evalcond[7]=(((gconst25*new_r11))+(((-1.0)*x151))+(((-1.0)*new_r01*x155)));
4509 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4510 vinfos[0].jointtype = 17;
4511 vinfos[0].foffset = j0;
4512 vinfos[0].indices[0] = _ij0[0];
4513 vinfos[0].indices[1] = _ij0[1];
4514 vinfos[0].maxsolutions = _nj0;
4515 vinfos[1].jointtype = 1;
4516 vinfos[1].foffset = j1;
4517 vinfos[1].indices[0] = _ij1[0];
4518 vinfos[1].indices[1] = _ij1[1];
4519 vinfos[1].maxsolutions = _nj1;
4520 vinfos[2].jointtype = 1;
4521 vinfos[2].foffset = j2;
4522 vinfos[2].indices[0] = _ij2[0];
4523 vinfos[2].indices[1] = _ij2[1];
4524 vinfos[2].maxsolutions = _nj2;
4525 vinfos[3].jointtype = 1;
4526 vinfos[3].foffset = j3;
4527 vinfos[3].indices[0] = _ij3[0];
4528 vinfos[3].indices[1] = _ij3[1];
4529 vinfos[3].maxsolutions = _nj3;
4530 vinfos[4].jointtype = 1;
4531 vinfos[4].foffset = j4;
4532 vinfos[4].indices[0] = _ij4[0];
4533 vinfos[4].indices[1] = _ij4[1];
4534 vinfos[4].maxsolutions = _nj4;
4535 vinfos[5].jointtype = 1;
4536 vinfos[5].foffset = j5;
4537 vinfos[5].indices[0] = _ij5[0];
4538 vinfos[5].indices[1] = _ij5[1];
4539 vinfos[5].maxsolutions = _nj5;
4540 vinfos[6].jointtype = 1;
4541 vinfos[6].foffset = j6;
4542 vinfos[6].indices[0] = _ij6[0];
4543 vinfos[6].indices[1] = _ij6[1];
4544 vinfos[6].maxsolutions = _nj6;
4545 std::vector<int> vfree(0);
4554 IkReal j6array[1], cj6array[1], sj6array[1];
4555 bool j6valid[1]={
false};
4565 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x156.
value)))+(x157.
value));
4566 sj6array[0]=
IKsin(j6array[0]);
4567 cj6array[0]=
IKcos(j6array[0]);
4568 if( j6array[0] >
IKPI )
4572 else if( j6array[0] < -
IKPI )
4573 { j6array[0]+=
IK2PI;
4576 for(
int ij6 = 0; ij6 < 1; ++ij6)
4582 _ij6[0] = ij6; _ij6[1] = -1;
4583 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4587 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4590 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4593 IkReal x158=
IKcos(j6);
4594 IkReal x159=
IKsin(j6);
4595 IkReal x160=((1.0)*gconst25);
4596 if((((1.0)+(((-1.0)*gconst25*x160)))) < -0.00001)
4598 IkReal x161=
IKsqrt(((1.0)+(((-1.0)*gconst25*x160))));
4599 IkReal x162=((1.0)*x161);
4601 evalcond[1]=((-1.0)*x159);
4602 evalcond[2]=((((-1.0)*x158*x160))+new_r11);
4603 evalcond[3]=((((-1.0)*x159*x160))+new_r10);
4604 evalcond[4]=(new_r01+((x158*x161)));
4605 evalcond[5]=(new_r00+((x159*x161)));
4606 evalcond[6]=((((-1.0)*new_r00*x162))+((gconst25*new_r10))+(((-1.0)*x159)));
4607 evalcond[7]=(((gconst25*new_r11))+(((-1.0)*x158))+(((-1.0)*new_r01*x162)));
4615 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4616 vinfos[0].jointtype = 17;
4617 vinfos[0].foffset = j0;
4618 vinfos[0].indices[0] = _ij0[0];
4619 vinfos[0].indices[1] = _ij0[1];
4620 vinfos[0].maxsolutions = _nj0;
4621 vinfos[1].jointtype = 1;
4622 vinfos[1].foffset = j1;
4623 vinfos[1].indices[0] = _ij1[0];
4624 vinfos[1].indices[1] = _ij1[1];
4625 vinfos[1].maxsolutions = _nj1;
4626 vinfos[2].jointtype = 1;
4627 vinfos[2].foffset = j2;
4628 vinfos[2].indices[0] = _ij2[0];
4629 vinfos[2].indices[1] = _ij2[1];
4630 vinfos[2].maxsolutions = _nj2;
4631 vinfos[3].jointtype = 1;
4632 vinfos[3].foffset = j3;
4633 vinfos[3].indices[0] = _ij3[0];
4634 vinfos[3].indices[1] = _ij3[1];
4635 vinfos[3].maxsolutions = _nj3;
4636 vinfos[4].jointtype = 1;
4637 vinfos[4].foffset = j4;
4638 vinfos[4].indices[0] = _ij4[0];
4639 vinfos[4].indices[1] = _ij4[1];
4640 vinfos[4].maxsolutions = _nj4;
4641 vinfos[5].jointtype = 1;
4642 vinfos[5].foffset = j5;
4643 vinfos[5].indices[0] = _ij5[0];
4644 vinfos[5].indices[1] = _ij5[1];
4645 vinfos[5].maxsolutions = _nj5;
4646 vinfos[6].jointtype = 1;
4647 vinfos[6].foffset = j6;
4648 vinfos[6].indices[0] = _ij6[0];
4649 vinfos[6].indices[1] = _ij6[1];
4650 vinfos[6].maxsolutions = _nj6;
4651 std::vector<int> vfree(0);
4663 if( bgotonextstatement )
4665 bool bgotonextstatement =
true;
4672 if((x163.
value) < -0.00001)
4675 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs((cj4+(((-1.0)*gconst25)))))+(
IKabs(((1.0)+(
IKsign(sj4)))))), 6.28318530717959)));
4676 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
4678 bgotonextstatement=
false;
4685 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
4687 sj4=((-1.0)*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25)))))));
4691 j4=((-1.0)*(
IKacos(gconst25)));
4696 if((x164.
value) < -0.00001)
4699 j6eval[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
4700 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
4703 IkReal j6array[1], cj6array[1], sj6array[1];
4704 bool j6valid[1]={
false};
4706 if((((1.0)+(((-1.0)*(gconst25*gconst25))))) < -0.00001)
4714 j6array[0]=
IKatan2((((gconst25*new_r10))+((new_r00*(
IKsqrt(((1.0)+(((-1.0)*(gconst25*gconst25))))))))), (new_r11*(x165.
value)));
4715 sj6array[0]=
IKsin(j6array[0]);
4716 cj6array[0]=
IKcos(j6array[0]);
4717 if( j6array[0] >
IKPI )
4721 else if( j6array[0] < -
IKPI )
4722 { j6array[0]+=
IK2PI;
4725 for(
int ij6 = 0; ij6 < 1; ++ij6)
4731 _ij6[0] = ij6; _ij6[1] = -1;
4732 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4736 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4739 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4742 IkReal x166=
IKcos(j6);
4743 IkReal x167=
IKsin(j6);
4744 IkReal x168=((1.0)*gconst25);
4745 IkReal x169=((1.0)*x166);
4746 IkReal x170=((1.0)*x167);
4747 if((((1.0)+(((-1.0)*gconst25*x168)))) < -0.00001)
4749 IkReal x171=
IKsqrt(((1.0)+(((-1.0)*gconst25*x168))));
4751 evalcond[1]=((-1.0)*x167);
4752 evalcond[2]=((((-1.0)*x166*x168))+new_r11);
4753 evalcond[3]=(new_r10+(((-1.0)*x167*x168)));
4754 evalcond[4]=(new_r01+(((-1.0)*x169*x171)));
4755 evalcond[5]=((((-1.0)*x170*x171))+new_r00);
4756 evalcond[6]=(((gconst25*new_r10))+((new_r00*x171))+(((-1.0)*x170)));
4757 evalcond[7]=(((new_r01*x171))+((gconst25*new_r11))+(((-1.0)*x169)));
4765 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4766 vinfos[0].jointtype = 17;
4767 vinfos[0].foffset = j0;
4768 vinfos[0].indices[0] = _ij0[0];
4769 vinfos[0].indices[1] = _ij0[1];
4770 vinfos[0].maxsolutions = _nj0;
4771 vinfos[1].jointtype = 1;
4772 vinfos[1].foffset = j1;
4773 vinfos[1].indices[0] = _ij1[0];
4774 vinfos[1].indices[1] = _ij1[1];
4775 vinfos[1].maxsolutions = _nj1;
4776 vinfos[2].jointtype = 1;
4777 vinfos[2].foffset = j2;
4778 vinfos[2].indices[0] = _ij2[0];
4779 vinfos[2].indices[1] = _ij2[1];
4780 vinfos[2].maxsolutions = _nj2;
4781 vinfos[3].jointtype = 1;
4782 vinfos[3].foffset = j3;
4783 vinfos[3].indices[0] = _ij3[0];
4784 vinfos[3].indices[1] = _ij3[1];
4785 vinfos[3].maxsolutions = _nj3;
4786 vinfos[4].jointtype = 1;
4787 vinfos[4].foffset = j4;
4788 vinfos[4].indices[0] = _ij4[0];
4789 vinfos[4].indices[1] = _ij4[1];
4790 vinfos[4].maxsolutions = _nj4;
4791 vinfos[5].jointtype = 1;
4792 vinfos[5].foffset = j5;
4793 vinfos[5].indices[0] = _ij5[0];
4794 vinfos[5].indices[1] = _ij5[1];
4795 vinfos[5].maxsolutions = _nj5;
4796 vinfos[6].jointtype = 1;
4797 vinfos[6].foffset = j6;
4798 vinfos[6].indices[0] = _ij6[0];
4799 vinfos[6].indices[1] = _ij6[1];
4800 vinfos[6].maxsolutions = _nj6;
4801 std::vector<int> vfree(0);
4810 IkReal j6array[1], cj6array[1], sj6array[1];
4811 bool j6valid[1]={
false};
4821 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x172.
value)))+(x173.
value));
4822 sj6array[0]=
IKsin(j6array[0]);
4823 cj6array[0]=
IKcos(j6array[0]);
4824 if( j6array[0] >
IKPI )
4828 else if( j6array[0] < -
IKPI )
4829 { j6array[0]+=
IK2PI;
4832 for(
int ij6 = 0; ij6 < 1; ++ij6)
4838 _ij6[0] = ij6; _ij6[1] = -1;
4839 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4843 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4846 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4849 IkReal x174=
IKcos(j6);
4850 IkReal x175=
IKsin(j6);
4851 IkReal x176=((1.0)*gconst25);
4852 IkReal x177=((1.0)*x174);
4853 IkReal x178=((1.0)*x175);
4854 if((((1.0)+(((-1.0)*gconst25*x176)))) < -0.00001)
4856 IkReal x179=
IKsqrt(((1.0)+(((-1.0)*gconst25*x176))));
4858 evalcond[1]=((-1.0)*x175);
4859 evalcond[2]=((((-1.0)*x174*x176))+new_r11);
4860 evalcond[3]=((((-1.0)*x175*x176))+new_r10);
4861 evalcond[4]=((((-1.0)*x177*x179))+new_r01);
4862 evalcond[5]=((((-1.0)*x178*x179))+new_r00);
4863 evalcond[6]=(((gconst25*new_r10))+((new_r00*x179))+(((-1.0)*x178)));
4864 evalcond[7]=(((new_r01*x179))+((gconst25*new_r11))+(((-1.0)*x177)));
4872 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
4873 vinfos[0].jointtype = 17;
4874 vinfos[0].foffset = j0;
4875 vinfos[0].indices[0] = _ij0[0];
4876 vinfos[0].indices[1] = _ij0[1];
4877 vinfos[0].maxsolutions = _nj0;
4878 vinfos[1].jointtype = 1;
4879 vinfos[1].foffset = j1;
4880 vinfos[1].indices[0] = _ij1[0];
4881 vinfos[1].indices[1] = _ij1[1];
4882 vinfos[1].maxsolutions = _nj1;
4883 vinfos[2].jointtype = 1;
4884 vinfos[2].foffset = j2;
4885 vinfos[2].indices[0] = _ij2[0];
4886 vinfos[2].indices[1] = _ij2[1];
4887 vinfos[2].maxsolutions = _nj2;
4888 vinfos[3].jointtype = 1;
4889 vinfos[3].foffset = j3;
4890 vinfos[3].indices[0] = _ij3[0];
4891 vinfos[3].indices[1] = _ij3[1];
4892 vinfos[3].maxsolutions = _nj3;
4893 vinfos[4].jointtype = 1;
4894 vinfos[4].foffset = j4;
4895 vinfos[4].indices[0] = _ij4[0];
4896 vinfos[4].indices[1] = _ij4[1];
4897 vinfos[4].maxsolutions = _nj4;
4898 vinfos[5].jointtype = 1;
4899 vinfos[5].foffset = j5;
4900 vinfos[5].indices[0] = _ij5[0];
4901 vinfos[5].indices[1] = _ij5[1];
4902 vinfos[5].maxsolutions = _nj5;
4903 vinfos[6].jointtype = 1;
4904 vinfos[6].foffset = j6;
4905 vinfos[6].indices[0] = _ij6[0];
4906 vinfos[6].indices[1] = _ij6[1];
4907 vinfos[6].maxsolutions = _nj6;
4908 std::vector<int> vfree(0);
4920 if( bgotonextstatement )
4922 bool bgotonextstatement =
true;
4927 bgotonextstatement=
false;
4932 if( bgotonextstatement )
4948 IkReal j6array[1], cj6array[1], sj6array[1];
4949 bool j6valid[1]={
false};
4951 IkReal x180=new_r22*new_r22;
4962 j6array[0]=
IKatan2(((x181.
value)*(((((-1.0)*new_r01*new_r22))+(((-1.0)*new_r10))))), ((x182.
value)*((((new_r10*new_r22))+new_r01))));
4963 sj6array[0]=
IKsin(j6array[0]);
4964 cj6array[0]=
IKcos(j6array[0]);
4965 if( j6array[0] >
IKPI )
4969 else if( j6array[0] < -
IKPI )
4970 { j6array[0]+=
IK2PI;
4973 for(
int ij6 = 0; ij6 < 1; ++ij6)
4979 _ij6[0] = ij6; _ij6[1] = -1;
4980 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
4984 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
4987 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
4989 IkReal evalcond[10];
4990 IkReal x183=
IKsin(j6);
4991 IkReal x184=
IKcos(j6);
4992 IkReal x185=(new_r22*sj4);
4993 IkReal x186=((1.0)*sj4);
4994 IkReal x187=((1.0)*new_r22);
4995 IkReal x188=(cj4*new_r01);
4996 IkReal x189=(cj4*new_r00);
4997 IkReal x190=((1.0)*x183);
4998 IkReal x191=((1.0)*x184);
4999 IkReal x192=(new_r22*x183);
5000 evalcond[0]=(((new_r11*sj4))+x192+x188);
5001 evalcond[1]=(((cj4*new_r10))+(((-1.0)*x190))+(((-1.0)*new_r00*x186)));
5002 evalcond[2]=(((cj4*new_r11))+(((-1.0)*x191))+(((-1.0)*new_r01*x186)));
5003 evalcond[3]=(((cj4*x192))+((sj4*x184))+new_r01);
5004 evalcond[4]=(((new_r10*sj4))+x189+(((-1.0)*x184*x187)));
5005 evalcond[5]=(((sj4*x183))+(((-1.0)*cj4*x184*x187))+new_r00);
5006 evalcond[6]=((((-1.0)*cj4*x191))+new_r11+((x183*x185)));
5007 evalcond[7]=((((-1.0)*x187*x189))+x184+(((-1.0)*new_r10*x185)));
5008 evalcond[8]=((((-1.0)*cj4*x190))+(((-1.0)*x185*x191))+new_r10);
5009 evalcond[9]=((((-1.0)*x190))+(((-1.0)*x187*x188))+(((-1.0)*new_r11*x185)));
5017 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5018 vinfos[0].jointtype = 17;
5019 vinfos[0].foffset = j0;
5020 vinfos[0].indices[0] = _ij0[0];
5021 vinfos[0].indices[1] = _ij0[1];
5022 vinfos[0].maxsolutions = _nj0;
5023 vinfos[1].jointtype = 1;
5024 vinfos[1].foffset = j1;
5025 vinfos[1].indices[0] = _ij1[0];
5026 vinfos[1].indices[1] = _ij1[1];
5027 vinfos[1].maxsolutions = _nj1;
5028 vinfos[2].jointtype = 1;
5029 vinfos[2].foffset = j2;
5030 vinfos[2].indices[0] = _ij2[0];
5031 vinfos[2].indices[1] = _ij2[1];
5032 vinfos[2].maxsolutions = _nj2;
5033 vinfos[3].jointtype = 1;
5034 vinfos[3].foffset = j3;
5035 vinfos[3].indices[0] = _ij3[0];
5036 vinfos[3].indices[1] = _ij3[1];
5037 vinfos[3].maxsolutions = _nj3;
5038 vinfos[4].jointtype = 1;
5039 vinfos[4].foffset = j4;
5040 vinfos[4].indices[0] = _ij4[0];
5041 vinfos[4].indices[1] = _ij4[1];
5042 vinfos[4].maxsolutions = _nj4;
5043 vinfos[5].jointtype = 1;
5044 vinfos[5].foffset = j5;
5045 vinfos[5].indices[0] = _ij5[0];
5046 vinfos[5].indices[1] = _ij5[1];
5047 vinfos[5].maxsolutions = _nj5;
5048 vinfos[6].jointtype = 1;
5049 vinfos[6].foffset = j6;
5050 vinfos[6].indices[0] = _ij6[0];
5051 vinfos[6].indices[1] = _ij6[1];
5052 vinfos[6].maxsolutions = _nj6;
5053 std::vector<int> vfree(0);
5066 IkReal j6array[1], cj6array[1], sj6array[1];
5067 bool j6valid[1]={
false};
5069 IkReal x193=((1.0)*sj4);
5076 j6array[0]=
IKatan2(((x194.
value)*(((((-1.0)*cj4*new_r01))+(((-1.0)*new_r11*x193))))), (((cj4*new_r11))+(((-1.0)*new_r01*x193))));
5077 sj6array[0]=
IKsin(j6array[0]);
5078 cj6array[0]=
IKcos(j6array[0]);
5079 if( j6array[0] >
IKPI )
5083 else if( j6array[0] < -
IKPI )
5084 { j6array[0]+=
IK2PI;
5087 for(
int ij6 = 0; ij6 < 1; ++ij6)
5093 _ij6[0] = ij6; _ij6[1] = -1;
5094 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5098 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5101 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5103 IkReal evalcond[10];
5104 IkReal x195=
IKsin(j6);
5105 IkReal x196=
IKcos(j6);
5106 IkReal x197=(new_r22*sj4);
5107 IkReal x198=((1.0)*sj4);
5108 IkReal x199=((1.0)*new_r22);
5109 IkReal x200=(cj4*new_r01);
5110 IkReal x201=(cj4*new_r00);
5111 IkReal x202=((1.0)*x195);
5112 IkReal x203=((1.0)*x196);
5113 IkReal x204=(new_r22*x195);
5114 evalcond[0]=(((new_r11*sj4))+x204+x200);
5115 evalcond[1]=(((cj4*new_r10))+(((-1.0)*x202))+(((-1.0)*new_r00*x198)));
5116 evalcond[2]=(((cj4*new_r11))+(((-1.0)*x203))+(((-1.0)*new_r01*x198)));
5117 evalcond[3]=(((cj4*x204))+((sj4*x196))+new_r01);
5118 evalcond[4]=((((-1.0)*x196*x199))+((new_r10*sj4))+x201);
5119 evalcond[5]=((((-1.0)*cj4*x196*x199))+((sj4*x195))+new_r00);
5120 evalcond[6]=((((-1.0)*cj4*x203))+((x195*x197))+new_r11);
5121 evalcond[7]=(x196+(((-1.0)*x199*x201))+(((-1.0)*new_r10*x197)));
5122 evalcond[8]=((((-1.0)*x197*x203))+(((-1.0)*cj4*x202))+new_r10);
5123 evalcond[9]=((((-1.0)*x202))+(((-1.0)*x199*x200))+(((-1.0)*new_r11*x197)));
5131 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5132 vinfos[0].jointtype = 17;
5133 vinfos[0].foffset = j0;
5134 vinfos[0].indices[0] = _ij0[0];
5135 vinfos[0].indices[1] = _ij0[1];
5136 vinfos[0].maxsolutions = _nj0;
5137 vinfos[1].jointtype = 1;
5138 vinfos[1].foffset = j1;
5139 vinfos[1].indices[0] = _ij1[0];
5140 vinfos[1].indices[1] = _ij1[1];
5141 vinfos[1].maxsolutions = _nj1;
5142 vinfos[2].jointtype = 1;
5143 vinfos[2].foffset = j2;
5144 vinfos[2].indices[0] = _ij2[0];
5145 vinfos[2].indices[1] = _ij2[1];
5146 vinfos[2].maxsolutions = _nj2;
5147 vinfos[3].jointtype = 1;
5148 vinfos[3].foffset = j3;
5149 vinfos[3].indices[0] = _ij3[0];
5150 vinfos[3].indices[1] = _ij3[1];
5151 vinfos[3].maxsolutions = _nj3;
5152 vinfos[4].jointtype = 1;
5153 vinfos[4].foffset = j4;
5154 vinfos[4].indices[0] = _ij4[0];
5155 vinfos[4].indices[1] = _ij4[1];
5156 vinfos[4].maxsolutions = _nj4;
5157 vinfos[5].jointtype = 1;
5158 vinfos[5].foffset = j5;
5159 vinfos[5].indices[0] = _ij5[0];
5160 vinfos[5].indices[1] = _ij5[1];
5161 vinfos[5].maxsolutions = _nj5;
5162 vinfos[6].jointtype = 1;
5163 vinfos[6].foffset = j6;
5164 vinfos[6].indices[0] = _ij6[0];
5165 vinfos[6].indices[1] = _ij6[1];
5166 vinfos[6].maxsolutions = _nj6;
5167 std::vector<int> vfree(0);
5180 IkReal j6array[1], cj6array[1], sj6array[1];
5181 bool j6valid[1]={
false};
5183 IkReal x205=cj4*cj4;
5184 IkReal x206=(cj4*new_r22);
5193 j6array[0]=((-1.5707963267949)+(x207.
value)+(((1.5707963267949)*(x208.
value))));
5194 sj6array[0]=
IKsin(j6array[0]);
5195 cj6array[0]=
IKcos(j6array[0]);
5196 if( j6array[0] >
IKPI )
5200 else if( j6array[0] < -
IKPI )
5201 { j6array[0]+=
IK2PI;
5204 for(
int ij6 = 0; ij6 < 1; ++ij6)
5210 _ij6[0] = ij6; _ij6[1] = -1;
5211 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5215 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5218 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5220 IkReal evalcond[10];
5221 IkReal x209=
IKsin(j6);
5222 IkReal x210=
IKcos(j6);
5223 IkReal x211=(new_r22*sj4);
5224 IkReal x212=((1.0)*sj4);
5225 IkReal x213=((1.0)*new_r22);
5226 IkReal x214=(cj4*new_r01);
5227 IkReal x215=(cj4*new_r00);
5228 IkReal x216=((1.0)*x209);
5229 IkReal x217=((1.0)*x210);
5230 IkReal x218=(new_r22*x209);
5231 evalcond[0]=(((new_r11*sj4))+x214+x218);
5232 evalcond[1]=(((cj4*new_r10))+(((-1.0)*x216))+(((-1.0)*new_r00*x212)));
5233 evalcond[2]=(((cj4*new_r11))+(((-1.0)*new_r01*x212))+(((-1.0)*x217)));
5234 evalcond[3]=(((cj4*x218))+new_r01+((sj4*x210)));
5235 evalcond[4]=(((new_r10*sj4))+x215+(((-1.0)*x210*x213)));
5236 evalcond[5]=(((sj4*x209))+(((-1.0)*cj4*x210*x213))+new_r00);
5237 evalcond[6]=(((x209*x211))+(((-1.0)*cj4*x217))+new_r11);
5238 evalcond[7]=(x210+(((-1.0)*new_r10*x211))+(((-1.0)*x213*x215)));
5239 evalcond[8]=((((-1.0)*cj4*x216))+(((-1.0)*x211*x217))+new_r10);
5240 evalcond[9]=((((-1.0)*new_r11*x211))+(((-1.0)*x213*x214))+(((-1.0)*x216)));
5248 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5249 vinfos[0].jointtype = 17;
5250 vinfos[0].foffset = j0;
5251 vinfos[0].indices[0] = _ij0[0];
5252 vinfos[0].indices[1] = _ij0[1];
5253 vinfos[0].maxsolutions = _nj0;
5254 vinfos[1].jointtype = 1;
5255 vinfos[1].foffset = j1;
5256 vinfos[1].indices[0] = _ij1[0];
5257 vinfos[1].indices[1] = _ij1[1];
5258 vinfos[1].maxsolutions = _nj1;
5259 vinfos[2].jointtype = 1;
5260 vinfos[2].foffset = j2;
5261 vinfos[2].indices[0] = _ij2[0];
5262 vinfos[2].indices[1] = _ij2[1];
5263 vinfos[2].maxsolutions = _nj2;
5264 vinfos[3].jointtype = 1;
5265 vinfos[3].foffset = j3;
5266 vinfos[3].indices[0] = _ij3[0];
5267 vinfos[3].indices[1] = _ij3[1];
5268 vinfos[3].maxsolutions = _nj3;
5269 vinfos[4].jointtype = 1;
5270 vinfos[4].foffset = j4;
5271 vinfos[4].indices[0] = _ij4[0];
5272 vinfos[4].indices[1] = _ij4[1];
5273 vinfos[4].maxsolutions = _nj4;
5274 vinfos[5].jointtype = 1;
5275 vinfos[5].foffset = j5;
5276 vinfos[5].indices[0] = _ij5[0];
5277 vinfos[5].indices[1] = _ij5[1];
5278 vinfos[5].maxsolutions = _nj5;
5279 vinfos[6].jointtype = 1;
5280 vinfos[6].foffset = j6;
5281 vinfos[6].indices[0] = _ij6[0];
5282 vinfos[6].indices[1] = _ij6[1];
5283 vinfos[6].maxsolutions = _nj6;
5284 std::vector<int> vfree(0);
5301 if( bgotonextstatement )
5303 bool bgotonextstatement =
true;
5308 bgotonextstatement=
false;
5313 if( bgotonextstatement )
5324 IkReal j4array[1], cj4array[1], sj4array[1];
5325 bool j4valid[1]={
false};
5331 IkReal x219=x220.
value;
5338 j4array[0]=
IKatan2((x219*(x221.
value)*(((1.0)+(((-1.0)*(new_r02*new_r02)))+(((-1.0)*(cj5*cj5)))))), (new_r02*x219));
5339 sj4array[0]=
IKsin(j4array[0]);
5340 cj4array[0]=
IKcos(j4array[0]);
5341 if( j4array[0] >
IKPI )
5345 else if( j4array[0] < -
IKPI )
5346 { j4array[0]+=
IK2PI;
5349 for(
int ij4 = 0; ij4 < 1; ++ij4)
5355 _ij4[0] = ij4; _ij4[1] = -1;
5356 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
5360 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
5363 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
5366 IkReal x222=
IKcos(j4);
5367 IkReal x223=
IKsin(j4);
5368 IkReal x224=((1.0)*sj5);
5369 IkReal x225=((1.0)*cj5);
5370 IkReal x226=(new_r12*x223);
5371 IkReal x227=(new_r02*x222);
5372 evalcond[0]=((((-1.0)*x222*x224))+new_r02);
5373 evalcond[1]=((((-1.0)*x223*x224))+new_r12);
5374 evalcond[2]=(((new_r12*x222))+(((-1.0)*new_r02*x223)));
5375 evalcond[3]=(x227+x226+(((-1.0)*x224)));
5376 evalcond[4]=((((-1.0)*x225*x226))+(((-1.0)*x225*x227))+((new_r22*sj5)));
5377 evalcond[5]=((((-1.0)*new_r10*x223*x224))+(((-1.0)*new_r20*x225))+(((-1.0)*new_r00*x222*x224)));
5378 evalcond[6]=((((-1.0)*new_r01*x222*x224))+(((-1.0)*new_r11*x223*x224))+(((-1.0)*new_r21*x225)));
5379 evalcond[7]=((1.0)+(((-1.0)*x224*x226))+(((-1.0)*x224*x227))+(((-1.0)*new_r22*x225)));
5389 j6eval[1]=((
IKabs(new_r20))+(
IKabs(new_r21)));
5391 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
5397 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
5404 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
5408 bool bgotonextstatement =
true;
5411 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
5412 evalcond[1]=new_r02;
5413 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
5415 bgotonextstatement=
false;
5422 j6eval[1]=
IKsign(new_r12);
5423 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
5424 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
5433 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
5434 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
5442 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
5446 bool bgotonextstatement =
true;
5449 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
5450 evalcond[1]=new_r22;
5451 evalcond[2]=new_r11;
5452 evalcond[3]=new_r10;
5453 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
5455 bgotonextstatement=
false;
5457 IkReal j6array[1], cj6array[1], sj6array[1];
5458 bool j6valid[1]={
false};
5462 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
5463 sj6array[0]=
IKsin(j6array[0]);
5464 cj6array[0]=
IKcos(j6array[0]);
5465 if( j6array[0] >
IKPI )
5469 else if( j6array[0] < -
IKPI )
5470 { j6array[0]+=
IK2PI;
5473 for(
int ij6 = 0; ij6 < 1; ++ij6)
5479 _ij6[0] = ij6; _ij6[1] = -1;
5480 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5484 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5487 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5490 IkReal x228=
IKcos(j6);
5491 IkReal x229=((1.0)*(
IKsin(j6)));
5492 evalcond[0]=(x228+new_r20);
5493 evalcond[1]=(new_r21+(((-1.0)*x229)));
5494 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x229)));
5495 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x228)));
5503 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5504 vinfos[0].jointtype = 17;
5505 vinfos[0].foffset = j0;
5506 vinfos[0].indices[0] = _ij0[0];
5507 vinfos[0].indices[1] = _ij0[1];
5508 vinfos[0].maxsolutions = _nj0;
5509 vinfos[1].jointtype = 1;
5510 vinfos[1].foffset = j1;
5511 vinfos[1].indices[0] = _ij1[0];
5512 vinfos[1].indices[1] = _ij1[1];
5513 vinfos[1].maxsolutions = _nj1;
5514 vinfos[2].jointtype = 1;
5515 vinfos[2].foffset = j2;
5516 vinfos[2].indices[0] = _ij2[0];
5517 vinfos[2].indices[1] = _ij2[1];
5518 vinfos[2].maxsolutions = _nj2;
5519 vinfos[3].jointtype = 1;
5520 vinfos[3].foffset = j3;
5521 vinfos[3].indices[0] = _ij3[0];
5522 vinfos[3].indices[1] = _ij3[1];
5523 vinfos[3].maxsolutions = _nj3;
5524 vinfos[4].jointtype = 1;
5525 vinfos[4].foffset = j4;
5526 vinfos[4].indices[0] = _ij4[0];
5527 vinfos[4].indices[1] = _ij4[1];
5528 vinfos[4].maxsolutions = _nj4;
5529 vinfos[5].jointtype = 1;
5530 vinfos[5].foffset = j5;
5531 vinfos[5].indices[0] = _ij5[0];
5532 vinfos[5].indices[1] = _ij5[1];
5533 vinfos[5].maxsolutions = _nj5;
5534 vinfos[6].jointtype = 1;
5535 vinfos[6].foffset = j6;
5536 vinfos[6].indices[0] = _ij6[0];
5537 vinfos[6].indices[1] = _ij6[1];
5538 vinfos[6].maxsolutions = _nj6;
5539 std::vector<int> vfree(0);
5547 if( bgotonextstatement )
5549 bool bgotonextstatement =
true;
5552 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
5553 evalcond[1]=new_r22;
5554 evalcond[2]=new_r11;
5555 evalcond[3]=new_r10;
5556 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
5558 bgotonextstatement=
false;
5560 IkReal j6array[1], cj6array[1], sj6array[1];
5561 bool j6valid[1]={
false};
5565 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
5566 sj6array[0]=
IKsin(j6array[0]);
5567 cj6array[0]=
IKcos(j6array[0]);
5568 if( j6array[0] >
IKPI )
5572 else if( j6array[0] < -
IKPI )
5573 { j6array[0]+=
IK2PI;
5576 for(
int ij6 = 0; ij6 < 1; ++ij6)
5582 _ij6[0] = ij6; _ij6[1] = -1;
5583 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5587 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5590 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5593 IkReal x230=
IKsin(j6);
5594 IkReal x231=((1.0)*(
IKcos(j6)));
5595 evalcond[0]=(x230+new_r21);
5596 evalcond[1]=((((-1.0)*x231))+new_r20);
5597 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x230)));
5598 evalcond[3]=((((-1.0)*x231))+(((-1.0)*new_r01)));
5606 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5607 vinfos[0].jointtype = 17;
5608 vinfos[0].foffset = j0;
5609 vinfos[0].indices[0] = _ij0[0];
5610 vinfos[0].indices[1] = _ij0[1];
5611 vinfos[0].maxsolutions = _nj0;
5612 vinfos[1].jointtype = 1;
5613 vinfos[1].foffset = j1;
5614 vinfos[1].indices[0] = _ij1[0];
5615 vinfos[1].indices[1] = _ij1[1];
5616 vinfos[1].maxsolutions = _nj1;
5617 vinfos[2].jointtype = 1;
5618 vinfos[2].foffset = j2;
5619 vinfos[2].indices[0] = _ij2[0];
5620 vinfos[2].indices[1] = _ij2[1];
5621 vinfos[2].maxsolutions = _nj2;
5622 vinfos[3].jointtype = 1;
5623 vinfos[3].foffset = j3;
5624 vinfos[3].indices[0] = _ij3[0];
5625 vinfos[3].indices[1] = _ij3[1];
5626 vinfos[3].maxsolutions = _nj3;
5627 vinfos[4].jointtype = 1;
5628 vinfos[4].foffset = j4;
5629 vinfos[4].indices[0] = _ij4[0];
5630 vinfos[4].indices[1] = _ij4[1];
5631 vinfos[4].maxsolutions = _nj4;
5632 vinfos[5].jointtype = 1;
5633 vinfos[5].foffset = j5;
5634 vinfos[5].indices[0] = _ij5[0];
5635 vinfos[5].indices[1] = _ij5[1];
5636 vinfos[5].maxsolutions = _nj5;
5637 vinfos[6].jointtype = 1;
5638 vinfos[6].foffset = j6;
5639 vinfos[6].indices[0] = _ij6[0];
5640 vinfos[6].indices[1] = _ij6[1];
5641 vinfos[6].maxsolutions = _nj6;
5642 std::vector<int> vfree(0);
5650 if( bgotonextstatement )
5652 bool bgotonextstatement =
true;
5655 evalcond[0]=
IKabs(new_r12);
5656 evalcond[1]=new_r20;
5657 evalcond[2]=new_r21;
5658 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
5660 bgotonextstatement=
false;
5669 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
5670 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
5679 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
5683 bool bgotonextstatement =
true;
5686 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
5687 evalcond[1]=new_r11;
5688 evalcond[2]=new_r10;
5689 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
5691 bgotonextstatement=
false;
5693 IkReal j6array[1], cj6array[1], sj6array[1];
5694 bool j6valid[1]={
false};
5698 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
5699 sj6array[0]=
IKsin(j6array[0]);
5700 cj6array[0]=
IKcos(j6array[0]);
5701 if( j6array[0] >
IKPI )
5705 else if( j6array[0] < -
IKPI )
5706 { j6array[0]+=
IK2PI;
5709 for(
int ij6 = 0; ij6 < 1; ++ij6)
5715 _ij6[0] = ij6; _ij6[1] = -1;
5716 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5720 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5723 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5726 IkReal x232=
IKsin(j6);
5727 IkReal x233=
IKcos(j6);
5729 evalcond[1]=((-1.0)*x232);
5730 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x232)));
5731 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x233)));
5739 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5740 vinfos[0].jointtype = 17;
5741 vinfos[0].foffset = j0;
5742 vinfos[0].indices[0] = _ij0[0];
5743 vinfos[0].indices[1] = _ij0[1];
5744 vinfos[0].maxsolutions = _nj0;
5745 vinfos[1].jointtype = 1;
5746 vinfos[1].foffset = j1;
5747 vinfos[1].indices[0] = _ij1[0];
5748 vinfos[1].indices[1] = _ij1[1];
5749 vinfos[1].maxsolutions = _nj1;
5750 vinfos[2].jointtype = 1;
5751 vinfos[2].foffset = j2;
5752 vinfos[2].indices[0] = _ij2[0];
5753 vinfos[2].indices[1] = _ij2[1];
5754 vinfos[2].maxsolutions = _nj2;
5755 vinfos[3].jointtype = 1;
5756 vinfos[3].foffset = j3;
5757 vinfos[3].indices[0] = _ij3[0];
5758 vinfos[3].indices[1] = _ij3[1];
5759 vinfos[3].maxsolutions = _nj3;
5760 vinfos[4].jointtype = 1;
5761 vinfos[4].foffset = j4;
5762 vinfos[4].indices[0] = _ij4[0];
5763 vinfos[4].indices[1] = _ij4[1];
5764 vinfos[4].maxsolutions = _nj4;
5765 vinfos[5].jointtype = 1;
5766 vinfos[5].foffset = j5;
5767 vinfos[5].indices[0] = _ij5[0];
5768 vinfos[5].indices[1] = _ij5[1];
5769 vinfos[5].maxsolutions = _nj5;
5770 vinfos[6].jointtype = 1;
5771 vinfos[6].foffset = j6;
5772 vinfos[6].indices[0] = _ij6[0];
5773 vinfos[6].indices[1] = _ij6[1];
5774 vinfos[6].maxsolutions = _nj6;
5775 std::vector<int> vfree(0);
5783 if( bgotonextstatement )
5785 bool bgotonextstatement =
true;
5788 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
5789 evalcond[1]=new_r11;
5790 evalcond[2]=new_r10;
5791 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
5793 bgotonextstatement=
false;
5795 IkReal j6array[1], cj6array[1], sj6array[1];
5796 bool j6valid[1]={
false};
5800 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
5801 sj6array[0]=
IKsin(j6array[0]);
5802 cj6array[0]=
IKcos(j6array[0]);
5803 if( j6array[0] >
IKPI )
5807 else if( j6array[0] < -
IKPI )
5808 { j6array[0]+=
IK2PI;
5811 for(
int ij6 = 0; ij6 < 1; ++ij6)
5817 _ij6[0] = ij6; _ij6[1] = -1;
5818 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5822 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5825 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5828 IkReal x234=
IKsin(j6);
5829 IkReal x235=
IKcos(j6);
5831 evalcond[1]=((-1.0)*x234);
5832 evalcond[2]=((((-1.0)*new_r00))+(((-1.0)*x234)));
5833 evalcond[3]=((((-1.0)*new_r01))+(((-1.0)*x235)));
5841 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5842 vinfos[0].jointtype = 17;
5843 vinfos[0].foffset = j0;
5844 vinfos[0].indices[0] = _ij0[0];
5845 vinfos[0].indices[1] = _ij0[1];
5846 vinfos[0].maxsolutions = _nj0;
5847 vinfos[1].jointtype = 1;
5848 vinfos[1].foffset = j1;
5849 vinfos[1].indices[0] = _ij1[0];
5850 vinfos[1].indices[1] = _ij1[1];
5851 vinfos[1].maxsolutions = _nj1;
5852 vinfos[2].jointtype = 1;
5853 vinfos[2].foffset = j2;
5854 vinfos[2].indices[0] = _ij2[0];
5855 vinfos[2].indices[1] = _ij2[1];
5856 vinfos[2].maxsolutions = _nj2;
5857 vinfos[3].jointtype = 1;
5858 vinfos[3].foffset = j3;
5859 vinfos[3].indices[0] = _ij3[0];
5860 vinfos[3].indices[1] = _ij3[1];
5861 vinfos[3].maxsolutions = _nj3;
5862 vinfos[4].jointtype = 1;
5863 vinfos[4].foffset = j4;
5864 vinfos[4].indices[0] = _ij4[0];
5865 vinfos[4].indices[1] = _ij4[1];
5866 vinfos[4].maxsolutions = _nj4;
5867 vinfos[5].jointtype = 1;
5868 vinfos[5].foffset = j5;
5869 vinfos[5].indices[0] = _ij5[0];
5870 vinfos[5].indices[1] = _ij5[1];
5871 vinfos[5].maxsolutions = _nj5;
5872 vinfos[6].jointtype = 1;
5873 vinfos[6].foffset = j6;
5874 vinfos[6].indices[0] = _ij6[0];
5875 vinfos[6].indices[1] = _ij6[1];
5876 vinfos[6].maxsolutions = _nj6;
5877 std::vector<int> vfree(0);
5885 if( bgotonextstatement )
5887 bool bgotonextstatement =
true;
5890 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
5891 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
5893 bgotonextstatement=
false;
5895 IkReal j6array[1], cj6array[1], sj6array[1];
5896 bool j6valid[1]={
false};
5900 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
5901 sj6array[0]=
IKsin(j6array[0]);
5902 cj6array[0]=
IKcos(j6array[0]);
5903 if( j6array[0] >
IKPI )
5907 else if( j6array[0] < -
IKPI )
5908 { j6array[0]+=
IK2PI;
5911 for(
int ij6 = 0; ij6 < 1; ++ij6)
5917 _ij6[0] = ij6; _ij6[1] = -1;
5918 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
5922 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
5925 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
5928 IkReal x236=
IKsin(j6);
5929 IkReal x237=
IKcos(j6);
5931 evalcond[1]=(cj5*x236);
5932 evalcond[2]=((-1.0)*x236);
5933 evalcond[3]=((-1.0)*cj5*x237);
5934 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x236)));
5935 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x237)));
5943 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5944 vinfos[0].jointtype = 17;
5945 vinfos[0].foffset = j0;
5946 vinfos[0].indices[0] = _ij0[0];
5947 vinfos[0].indices[1] = _ij0[1];
5948 vinfos[0].maxsolutions = _nj0;
5949 vinfos[1].jointtype = 1;
5950 vinfos[1].foffset = j1;
5951 vinfos[1].indices[0] = _ij1[0];
5952 vinfos[1].indices[1] = _ij1[1];
5953 vinfos[1].maxsolutions = _nj1;
5954 vinfos[2].jointtype = 1;
5955 vinfos[2].foffset = j2;
5956 vinfos[2].indices[0] = _ij2[0];
5957 vinfos[2].indices[1] = _ij2[1];
5958 vinfos[2].maxsolutions = _nj2;
5959 vinfos[3].jointtype = 1;
5960 vinfos[3].foffset = j3;
5961 vinfos[3].indices[0] = _ij3[0];
5962 vinfos[3].indices[1] = _ij3[1];
5963 vinfos[3].maxsolutions = _nj3;
5964 vinfos[4].jointtype = 1;
5965 vinfos[4].foffset = j4;
5966 vinfos[4].indices[0] = _ij4[0];
5967 vinfos[4].indices[1] = _ij4[1];
5968 vinfos[4].maxsolutions = _nj4;
5969 vinfos[5].jointtype = 1;
5970 vinfos[5].foffset = j5;
5971 vinfos[5].indices[0] = _ij5[0];
5972 vinfos[5].indices[1] = _ij5[1];
5973 vinfos[5].maxsolutions = _nj5;
5974 vinfos[6].jointtype = 1;
5975 vinfos[6].foffset = j6;
5976 vinfos[6].indices[0] = _ij6[0];
5977 vinfos[6].indices[1] = _ij6[1];
5978 vinfos[6].maxsolutions = _nj6;
5979 std::vector<int> vfree(0);
5987 if( bgotonextstatement )
5989 bool bgotonextstatement =
true;
5994 bgotonextstatement=
false;
5999 if( bgotonextstatement )
6010 IkReal j6array[1], cj6array[1], sj6array[1];
6011 bool j6valid[1]={
false};
6019 j6array[0]=
IKatan2(((-1.0)*new_r00), (new_r10*(x238.
value)));
6020 sj6array[0]=
IKsin(j6array[0]);
6021 cj6array[0]=
IKcos(j6array[0]);
6022 if( j6array[0] >
IKPI )
6026 else if( j6array[0] < -
IKPI )
6027 { j6array[0]+=
IK2PI;
6030 for(
int ij6 = 0; ij6 < 1; ++ij6)
6036 _ij6[0] = ij6; _ij6[1] = -1;
6037 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6041 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6044 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6047 IkReal x239=
IKcos(j6);
6048 IkReal x240=
IKsin(j6);
6049 IkReal x241=((1.0)*cj5);
6050 IkReal x242=((1.0)*x240);
6051 evalcond[0]=(((cj5*x240))+new_r11);
6052 evalcond[1]=(new_r10+(((-1.0)*x239*x241)));
6053 evalcond[2]=(x239+(((-1.0)*new_r10*x241)));
6054 evalcond[3]=((((-1.0)*new_r00))+(((-1.0)*x242)));
6055 evalcond[4]=((((-1.0)*new_r01))+(((-1.0)*x239)));
6056 evalcond[5]=((((-1.0)*new_r11*x241))+(((-1.0)*x242)));
6064 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6065 vinfos[0].jointtype = 17;
6066 vinfos[0].foffset = j0;
6067 vinfos[0].indices[0] = _ij0[0];
6068 vinfos[0].indices[1] = _ij0[1];
6069 vinfos[0].maxsolutions = _nj0;
6070 vinfos[1].jointtype = 1;
6071 vinfos[1].foffset = j1;
6072 vinfos[1].indices[0] = _ij1[0];
6073 vinfos[1].indices[1] = _ij1[1];
6074 vinfos[1].maxsolutions = _nj1;
6075 vinfos[2].jointtype = 1;
6076 vinfos[2].foffset = j2;
6077 vinfos[2].indices[0] = _ij2[0];
6078 vinfos[2].indices[1] = _ij2[1];
6079 vinfos[2].maxsolutions = _nj2;
6080 vinfos[3].jointtype = 1;
6081 vinfos[3].foffset = j3;
6082 vinfos[3].indices[0] = _ij3[0];
6083 vinfos[3].indices[1] = _ij3[1];
6084 vinfos[3].maxsolutions = _nj3;
6085 vinfos[4].jointtype = 1;
6086 vinfos[4].foffset = j4;
6087 vinfos[4].indices[0] = _ij4[0];
6088 vinfos[4].indices[1] = _ij4[1];
6089 vinfos[4].maxsolutions = _nj4;
6090 vinfos[5].jointtype = 1;
6091 vinfos[5].foffset = j5;
6092 vinfos[5].indices[0] = _ij5[0];
6093 vinfos[5].indices[1] = _ij5[1];
6094 vinfos[5].maxsolutions = _nj5;
6095 vinfos[6].jointtype = 1;
6096 vinfos[6].foffset = j6;
6097 vinfos[6].indices[0] = _ij6[0];
6098 vinfos[6].indices[1] = _ij6[1];
6099 vinfos[6].maxsolutions = _nj6;
6100 std::vector<int> vfree(0);
6113 IkReal j6array[1], cj6array[1], sj6array[1];
6114 bool j6valid[1]={
false};
6124 j6array[0]=((-1.5707963267949)+(x243.
value)+(((1.5707963267949)*(x244.
value))));
6125 sj6array[0]=
IKsin(j6array[0]);
6126 cj6array[0]=
IKcos(j6array[0]);
6127 if( j6array[0] >
IKPI )
6131 else if( j6array[0] < -
IKPI )
6132 { j6array[0]+=
IK2PI;
6135 for(
int ij6 = 0; ij6 < 1; ++ij6)
6141 _ij6[0] = ij6; _ij6[1] = -1;
6142 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6146 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6149 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6152 IkReal x245=
IKcos(j6);
6153 IkReal x246=
IKsin(j6);
6154 IkReal x247=((1.0)*cj5);
6155 IkReal x248=((1.0)*x246);
6156 evalcond[0]=(((cj5*x246))+new_r11);
6157 evalcond[1]=((((-1.0)*x245*x247))+new_r10);
6158 evalcond[2]=(x245+(((-1.0)*new_r10*x247)));
6159 evalcond[3]=((((-1.0)*new_r00))+(((-1.0)*x248)));
6160 evalcond[4]=((((-1.0)*x245))+(((-1.0)*new_r01)));
6161 evalcond[5]=((((-1.0)*new_r11*x247))+(((-1.0)*x248)));
6169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6170 vinfos[0].jointtype = 17;
6171 vinfos[0].foffset = j0;
6172 vinfos[0].indices[0] = _ij0[0];
6173 vinfos[0].indices[1] = _ij0[1];
6174 vinfos[0].maxsolutions = _nj0;
6175 vinfos[1].jointtype = 1;
6176 vinfos[1].foffset = j1;
6177 vinfos[1].indices[0] = _ij1[0];
6178 vinfos[1].indices[1] = _ij1[1];
6179 vinfos[1].maxsolutions = _nj1;
6180 vinfos[2].jointtype = 1;
6181 vinfos[2].foffset = j2;
6182 vinfos[2].indices[0] = _ij2[0];
6183 vinfos[2].indices[1] = _ij2[1];
6184 vinfos[2].maxsolutions = _nj2;
6185 vinfos[3].jointtype = 1;
6186 vinfos[3].foffset = j3;
6187 vinfos[3].indices[0] = _ij3[0];
6188 vinfos[3].indices[1] = _ij3[1];
6189 vinfos[3].maxsolutions = _nj3;
6190 vinfos[4].jointtype = 1;
6191 vinfos[4].foffset = j4;
6192 vinfos[4].indices[0] = _ij4[0];
6193 vinfos[4].indices[1] = _ij4[1];
6194 vinfos[4].maxsolutions = _nj4;
6195 vinfos[5].jointtype = 1;
6196 vinfos[5].foffset = j5;
6197 vinfos[5].indices[0] = _ij5[0];
6198 vinfos[5].indices[1] = _ij5[1];
6199 vinfos[5].maxsolutions = _nj5;
6200 vinfos[6].jointtype = 1;
6201 vinfos[6].foffset = j6;
6202 vinfos[6].indices[0] = _ij6[0];
6203 vinfos[6].indices[1] = _ij6[1];
6204 vinfos[6].maxsolutions = _nj6;
6205 std::vector<int> vfree(0);
6217 if( bgotonextstatement )
6219 bool bgotonextstatement =
true;
6222 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
6223 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
6225 bgotonextstatement=
false;
6227 IkReal j6array[1], cj6array[1], sj6array[1];
6228 bool j6valid[1]={
false};
6232 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
6233 sj6array[0]=
IKsin(j6array[0]);
6234 cj6array[0]=
IKcos(j6array[0]);
6235 if( j6array[0] >
IKPI )
6239 else if( j6array[0] < -
IKPI )
6240 { j6array[0]+=
IK2PI;
6243 for(
int ij6 = 0; ij6 < 1; ++ij6)
6249 _ij6[0] = ij6; _ij6[1] = -1;
6250 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6254 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6257 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6260 IkReal x249=
IKsin(j6);
6261 IkReal x250=
IKcos(j6);
6263 evalcond[1]=(new_r22*x249);
6264 evalcond[2]=((-1.0)*x249);
6265 evalcond[3]=((-1.0)*new_r22*x250);
6266 evalcond[4]=((((-1.0)*x249))+(((-1.0)*new_r00)));
6267 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x250)));
6275 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6276 vinfos[0].jointtype = 17;
6277 vinfos[0].foffset = j0;
6278 vinfos[0].indices[0] = _ij0[0];
6279 vinfos[0].indices[1] = _ij0[1];
6280 vinfos[0].maxsolutions = _nj0;
6281 vinfos[1].jointtype = 1;
6282 vinfos[1].foffset = j1;
6283 vinfos[1].indices[0] = _ij1[0];
6284 vinfos[1].indices[1] = _ij1[1];
6285 vinfos[1].maxsolutions = _nj1;
6286 vinfos[2].jointtype = 1;
6287 vinfos[2].foffset = j2;
6288 vinfos[2].indices[0] = _ij2[0];
6289 vinfos[2].indices[1] = _ij2[1];
6290 vinfos[2].maxsolutions = _nj2;
6291 vinfos[3].jointtype = 1;
6292 vinfos[3].foffset = j3;
6293 vinfos[3].indices[0] = _ij3[0];
6294 vinfos[3].indices[1] = _ij3[1];
6295 vinfos[3].maxsolutions = _nj3;
6296 vinfos[4].jointtype = 1;
6297 vinfos[4].foffset = j4;
6298 vinfos[4].indices[0] = _ij4[0];
6299 vinfos[4].indices[1] = _ij4[1];
6300 vinfos[4].maxsolutions = _nj4;
6301 vinfos[5].jointtype = 1;
6302 vinfos[5].foffset = j5;
6303 vinfos[5].indices[0] = _ij5[0];
6304 vinfos[5].indices[1] = _ij5[1];
6305 vinfos[5].maxsolutions = _nj5;
6306 vinfos[6].jointtype = 1;
6307 vinfos[6].foffset = j6;
6308 vinfos[6].indices[0] = _ij6[0];
6309 vinfos[6].indices[1] = _ij6[1];
6310 vinfos[6].maxsolutions = _nj6;
6311 std::vector<int> vfree(0);
6319 if( bgotonextstatement )
6321 bool bgotonextstatement =
true;
6326 bgotonextstatement=
false;
6331 if( bgotonextstatement )
6343 IkReal j6array[1], cj6array[1], sj6array[1];
6344 bool j6valid[1]={
false};
6352 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x251.
value)));
6353 sj6array[0]=
IKsin(j6array[0]);
6354 cj6array[0]=
IKcos(j6array[0]);
6355 if( j6array[0] >
IKPI )
6359 else if( j6array[0] < -
IKPI )
6360 { j6array[0]+=
IK2PI;
6363 for(
int ij6 = 0; ij6 < 1; ++ij6)
6369 _ij6[0] = ij6; _ij6[1] = -1;
6370 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6374 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6377 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6380 IkReal x252=
IKsin(j6);
6381 IkReal x253=
IKcos(j6);
6382 IkReal x254=((1.0)*cj5);
6383 IkReal x255=((1.0)*x252);
6384 evalcond[0]=(((new_r12*x253))+new_r20);
6385 evalcond[1]=(((cj5*x252))+new_r11);
6386 evalcond[2]=(new_r21+(((-1.0)*new_r12*x255)));
6387 evalcond[3]=(new_r10+(((-1.0)*x253*x254)));
6388 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x255)));
6389 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x253)));
6390 evalcond[6]=((((-1.0)*new_r10*x254))+((new_r20*sj5))+x253);
6391 evalcond[7]=((((-1.0)*new_r11*x254))+((new_r21*sj5))+(((-1.0)*x255)));
6399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6400 vinfos[0].jointtype = 17;
6401 vinfos[0].foffset = j0;
6402 vinfos[0].indices[0] = _ij0[0];
6403 vinfos[0].indices[1] = _ij0[1];
6404 vinfos[0].maxsolutions = _nj0;
6405 vinfos[1].jointtype = 1;
6406 vinfos[1].foffset = j1;
6407 vinfos[1].indices[0] = _ij1[0];
6408 vinfos[1].indices[1] = _ij1[1];
6409 vinfos[1].maxsolutions = _nj1;
6410 vinfos[2].jointtype = 1;
6411 vinfos[2].foffset = j2;
6412 vinfos[2].indices[0] = _ij2[0];
6413 vinfos[2].indices[1] = _ij2[1];
6414 vinfos[2].maxsolutions = _nj2;
6415 vinfos[3].jointtype = 1;
6416 vinfos[3].foffset = j3;
6417 vinfos[3].indices[0] = _ij3[0];
6418 vinfos[3].indices[1] = _ij3[1];
6419 vinfos[3].maxsolutions = _nj3;
6420 vinfos[4].jointtype = 1;
6421 vinfos[4].foffset = j4;
6422 vinfos[4].indices[0] = _ij4[0];
6423 vinfos[4].indices[1] = _ij4[1];
6424 vinfos[4].maxsolutions = _nj4;
6425 vinfos[5].jointtype = 1;
6426 vinfos[5].foffset = j5;
6427 vinfos[5].indices[0] = _ij5[0];
6428 vinfos[5].indices[1] = _ij5[1];
6429 vinfos[5].maxsolutions = _nj5;
6430 vinfos[6].jointtype = 1;
6431 vinfos[6].foffset = j6;
6432 vinfos[6].indices[0] = _ij6[0];
6433 vinfos[6].indices[1] = _ij6[1];
6434 vinfos[6].maxsolutions = _nj6;
6435 std::vector<int> vfree(0);
6448 IkReal j6array[1], cj6array[1], sj6array[1];
6449 bool j6valid[1]={
false};
6459 j6array[0]=((-1.5707963267949)+(x256.
value)+(((1.5707963267949)*(x257.
value))));
6460 sj6array[0]=
IKsin(j6array[0]);
6461 cj6array[0]=
IKcos(j6array[0]);
6462 if( j6array[0] >
IKPI )
6466 else if( j6array[0] < -
IKPI )
6467 { j6array[0]+=
IK2PI;
6470 for(
int ij6 = 0; ij6 < 1; ++ij6)
6476 _ij6[0] = ij6; _ij6[1] = -1;
6477 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6481 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6484 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6487 IkReal x258=
IKsin(j6);
6488 IkReal x259=
IKcos(j6);
6489 IkReal x260=((1.0)*cj5);
6490 IkReal x261=((1.0)*x258);
6491 evalcond[0]=(((new_r12*x259))+new_r20);
6492 evalcond[1]=(((cj5*x258))+new_r11);
6493 evalcond[2]=((((-1.0)*new_r12*x261))+new_r21);
6494 evalcond[3]=((((-1.0)*x259*x260))+new_r10);
6495 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x261)));
6496 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x259)));
6497 evalcond[6]=(((new_r20*sj5))+x259+(((-1.0)*new_r10*x260)));
6498 evalcond[7]=((((-1.0)*new_r11*x260))+((new_r21*sj5))+(((-1.0)*x261)));
6506 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6507 vinfos[0].jointtype = 17;
6508 vinfos[0].foffset = j0;
6509 vinfos[0].indices[0] = _ij0[0];
6510 vinfos[0].indices[1] = _ij0[1];
6511 vinfos[0].maxsolutions = _nj0;
6512 vinfos[1].jointtype = 1;
6513 vinfos[1].foffset = j1;
6514 vinfos[1].indices[0] = _ij1[0];
6515 vinfos[1].indices[1] = _ij1[1];
6516 vinfos[1].maxsolutions = _nj1;
6517 vinfos[2].jointtype = 1;
6518 vinfos[2].foffset = j2;
6519 vinfos[2].indices[0] = _ij2[0];
6520 vinfos[2].indices[1] = _ij2[1];
6521 vinfos[2].maxsolutions = _nj2;
6522 vinfos[3].jointtype = 1;
6523 vinfos[3].foffset = j3;
6524 vinfos[3].indices[0] = _ij3[0];
6525 vinfos[3].indices[1] = _ij3[1];
6526 vinfos[3].maxsolutions = _nj3;
6527 vinfos[4].jointtype = 1;
6528 vinfos[4].foffset = j4;
6529 vinfos[4].indices[0] = _ij4[0];
6530 vinfos[4].indices[1] = _ij4[1];
6531 vinfos[4].maxsolutions = _nj4;
6532 vinfos[5].jointtype = 1;
6533 vinfos[5].foffset = j5;
6534 vinfos[5].indices[0] = _ij5[0];
6535 vinfos[5].indices[1] = _ij5[1];
6536 vinfos[5].maxsolutions = _nj5;
6537 vinfos[6].jointtype = 1;
6538 vinfos[6].foffset = j6;
6539 vinfos[6].indices[0] = _ij6[0];
6540 vinfos[6].indices[1] = _ij6[1];
6541 vinfos[6].maxsolutions = _nj6;
6542 std::vector<int> vfree(0);
6555 IkReal j6array[1], cj6array[1], sj6array[1];
6556 bool j6valid[1]={
false};
6566 j6array[0]=((-1.5707963267949)+(x262.
value)+(((1.5707963267949)*(x263.
value))));
6567 sj6array[0]=
IKsin(j6array[0]);
6568 cj6array[0]=
IKcos(j6array[0]);
6569 if( j6array[0] >
IKPI )
6573 else if( j6array[0] < -
IKPI )
6574 { j6array[0]+=
IK2PI;
6577 for(
int ij6 = 0; ij6 < 1; ++ij6)
6583 _ij6[0] = ij6; _ij6[1] = -1;
6584 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6588 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6591 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6594 IkReal x264=
IKsin(j6);
6595 IkReal x265=
IKcos(j6);
6596 IkReal x266=((1.0)*cj5);
6597 IkReal x267=((1.0)*x264);
6598 evalcond[0]=(((new_r12*x265))+new_r20);
6599 evalcond[1]=(((cj5*x264))+new_r11);
6600 evalcond[2]=((((-1.0)*new_r12*x267))+new_r21);
6601 evalcond[3]=((((-1.0)*x265*x266))+new_r10);
6602 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x267)));
6603 evalcond[5]=((((-1.0)*x265))+(((-1.0)*new_r01)));
6604 evalcond[6]=(((new_r20*sj5))+x265+(((-1.0)*new_r10*x266)));
6605 evalcond[7]=((((-1.0)*new_r11*x266))+((new_r21*sj5))+(((-1.0)*x267)));
6613 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6614 vinfos[0].jointtype = 17;
6615 vinfos[0].foffset = j0;
6616 vinfos[0].indices[0] = _ij0[0];
6617 vinfos[0].indices[1] = _ij0[1];
6618 vinfos[0].maxsolutions = _nj0;
6619 vinfos[1].jointtype = 1;
6620 vinfos[1].foffset = j1;
6621 vinfos[1].indices[0] = _ij1[0];
6622 vinfos[1].indices[1] = _ij1[1];
6623 vinfos[1].maxsolutions = _nj1;
6624 vinfos[2].jointtype = 1;
6625 vinfos[2].foffset = j2;
6626 vinfos[2].indices[0] = _ij2[0];
6627 vinfos[2].indices[1] = _ij2[1];
6628 vinfos[2].maxsolutions = _nj2;
6629 vinfos[3].jointtype = 1;
6630 vinfos[3].foffset = j3;
6631 vinfos[3].indices[0] = _ij3[0];
6632 vinfos[3].indices[1] = _ij3[1];
6633 vinfos[3].maxsolutions = _nj3;
6634 vinfos[4].jointtype = 1;
6635 vinfos[4].foffset = j4;
6636 vinfos[4].indices[0] = _ij4[0];
6637 vinfos[4].indices[1] = _ij4[1];
6638 vinfos[4].maxsolutions = _nj4;
6639 vinfos[5].jointtype = 1;
6640 vinfos[5].foffset = j5;
6641 vinfos[5].indices[0] = _ij5[0];
6642 vinfos[5].indices[1] = _ij5[1];
6643 vinfos[5].maxsolutions = _nj5;
6644 vinfos[6].jointtype = 1;
6645 vinfos[6].foffset = j6;
6646 vinfos[6].indices[0] = _ij6[0];
6647 vinfos[6].indices[1] = _ij6[1];
6648 vinfos[6].maxsolutions = _nj6;
6649 std::vector<int> vfree(0);
6661 if( bgotonextstatement )
6663 bool bgotonextstatement =
true;
6666 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
6667 evalcond[1]=new_r02;
6668 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
6670 bgotonextstatement=
false;
6672 IkReal j6array[1], cj6array[1], sj6array[1];
6673 bool j6valid[1]={
false};
6677 j6array[0]=
IKatan2(new_r00, new_r01);
6678 sj6array[0]=
IKsin(j6array[0]);
6679 cj6array[0]=
IKcos(j6array[0]);
6680 if( j6array[0] >
IKPI )
6684 else if( j6array[0] < -
IKPI )
6685 { j6array[0]+=
IK2PI;
6688 for(
int ij6 = 0; ij6 < 1; ++ij6)
6694 _ij6[0] = ij6; _ij6[1] = -1;
6695 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6699 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6702 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6705 IkReal x268=
IKsin(j6);
6706 IkReal x269=
IKcos(j6);
6707 IkReal x270=((1.0)*x268);
6708 IkReal x271=((1.0)*x269);
6709 evalcond[0]=(((new_r12*x268))+new_r21);
6710 evalcond[1]=((((-1.0)*x270))+new_r00);
6711 evalcond[2]=((((-1.0)*x271))+new_r01);
6712 evalcond[3]=((((-1.0)*new_r12*x271))+new_r20);
6713 evalcond[4]=(((cj5*x268))+(((-1.0)*new_r11)));
6714 evalcond[5]=((((-1.0)*cj5*x271))+(((-1.0)*new_r10)));
6715 evalcond[6]=(((new_r20*sj5))+((cj5*new_r10))+x269);
6716 evalcond[7]=(((cj5*new_r11))+(((-1.0)*x270))+((new_r21*sj5)));
6724 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6725 vinfos[0].jointtype = 17;
6726 vinfos[0].foffset = j0;
6727 vinfos[0].indices[0] = _ij0[0];
6728 vinfos[0].indices[1] = _ij0[1];
6729 vinfos[0].maxsolutions = _nj0;
6730 vinfos[1].jointtype = 1;
6731 vinfos[1].foffset = j1;
6732 vinfos[1].indices[0] = _ij1[0];
6733 vinfos[1].indices[1] = _ij1[1];
6734 vinfos[1].maxsolutions = _nj1;
6735 vinfos[2].jointtype = 1;
6736 vinfos[2].foffset = j2;
6737 vinfos[2].indices[0] = _ij2[0];
6738 vinfos[2].indices[1] = _ij2[1];
6739 vinfos[2].maxsolutions = _nj2;
6740 vinfos[3].jointtype = 1;
6741 vinfos[3].foffset = j3;
6742 vinfos[3].indices[0] = _ij3[0];
6743 vinfos[3].indices[1] = _ij3[1];
6744 vinfos[3].maxsolutions = _nj3;
6745 vinfos[4].jointtype = 1;
6746 vinfos[4].foffset = j4;
6747 vinfos[4].indices[0] = _ij4[0];
6748 vinfos[4].indices[1] = _ij4[1];
6749 vinfos[4].maxsolutions = _nj4;
6750 vinfos[5].jointtype = 1;
6751 vinfos[5].foffset = j5;
6752 vinfos[5].indices[0] = _ij5[0];
6753 vinfos[5].indices[1] = _ij5[1];
6754 vinfos[5].maxsolutions = _nj5;
6755 vinfos[6].jointtype = 1;
6756 vinfos[6].foffset = j6;
6757 vinfos[6].indices[0] = _ij6[0];
6758 vinfos[6].indices[1] = _ij6[1];
6759 vinfos[6].maxsolutions = _nj6;
6760 std::vector<int> vfree(0);
6768 if( bgotonextstatement )
6770 bool bgotonextstatement =
true;
6773 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
6774 evalcond[1]=new_r22;
6775 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
6777 bgotonextstatement=
false;
6779 IkReal j6array[1], cj6array[1], sj6array[1];
6780 bool j6valid[1]={
false};
6784 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
6785 sj6array[0]=
IKsin(j6array[0]);
6786 cj6array[0]=
IKcos(j6array[0]);
6787 if( j6array[0] >
IKPI )
6791 else if( j6array[0] < -
IKPI )
6792 { j6array[0]+=
IK2PI;
6795 for(
int ij6 = 0; ij6 < 1; ++ij6)
6801 _ij6[0] = ij6; _ij6[1] = -1;
6802 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6806 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6809 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6812 IkReal x272=
IKcos(j6);
6813 IkReal x273=
IKsin(j6);
6814 IkReal x274=((1.0)*sj4);
6815 IkReal x275=((1.0)*x273);
6816 IkReal x276=((1.0)*x272);
6817 evalcond[0]=(x272+new_r20);
6818 evalcond[1]=((((-1.0)*x275))+new_r21);
6819 evalcond[2]=(new_r01+((sj4*x272)));
6820 evalcond[3]=(new_r00+((sj4*x273)));
6821 evalcond[4]=((((-1.0)*cj4*x276))+new_r11);
6822 evalcond[5]=((((-1.0)*cj4*x275))+new_r10);
6823 evalcond[6]=(((cj4*new_r10))+(((-1.0)*new_r00*x274))+(((-1.0)*x275)));
6824 evalcond[7]=((((-1.0)*new_r01*x274))+((cj4*new_r11))+(((-1.0)*x276)));
6832 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6833 vinfos[0].jointtype = 17;
6834 vinfos[0].foffset = j0;
6835 vinfos[0].indices[0] = _ij0[0];
6836 vinfos[0].indices[1] = _ij0[1];
6837 vinfos[0].maxsolutions = _nj0;
6838 vinfos[1].jointtype = 1;
6839 vinfos[1].foffset = j1;
6840 vinfos[1].indices[0] = _ij1[0];
6841 vinfos[1].indices[1] = _ij1[1];
6842 vinfos[1].maxsolutions = _nj1;
6843 vinfos[2].jointtype = 1;
6844 vinfos[2].foffset = j2;
6845 vinfos[2].indices[0] = _ij2[0];
6846 vinfos[2].indices[1] = _ij2[1];
6847 vinfos[2].maxsolutions = _nj2;
6848 vinfos[3].jointtype = 1;
6849 vinfos[3].foffset = j3;
6850 vinfos[3].indices[0] = _ij3[0];
6851 vinfos[3].indices[1] = _ij3[1];
6852 vinfos[3].maxsolutions = _nj3;
6853 vinfos[4].jointtype = 1;
6854 vinfos[4].foffset = j4;
6855 vinfos[4].indices[0] = _ij4[0];
6856 vinfos[4].indices[1] = _ij4[1];
6857 vinfos[4].maxsolutions = _nj4;
6858 vinfos[5].jointtype = 1;
6859 vinfos[5].foffset = j5;
6860 vinfos[5].indices[0] = _ij5[0];
6861 vinfos[5].indices[1] = _ij5[1];
6862 vinfos[5].maxsolutions = _nj5;
6863 vinfos[6].jointtype = 1;
6864 vinfos[6].foffset = j6;
6865 vinfos[6].indices[0] = _ij6[0];
6866 vinfos[6].indices[1] = _ij6[1];
6867 vinfos[6].maxsolutions = _nj6;
6868 std::vector<int> vfree(0);
6876 if( bgotonextstatement )
6878 bool bgotonextstatement =
true;
6881 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
6882 evalcond[1]=new_r22;
6883 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
6885 bgotonextstatement=
false;
6887 IkReal j6array[1], cj6array[1], sj6array[1];
6888 bool j6valid[1]={
false};
6892 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
6893 sj6array[0]=
IKsin(j6array[0]);
6894 cj6array[0]=
IKcos(j6array[0]);
6895 if( j6array[0] >
IKPI )
6899 else if( j6array[0] < -
IKPI )
6900 { j6array[0]+=
IK2PI;
6903 for(
int ij6 = 0; ij6 < 1; ++ij6)
6909 _ij6[0] = ij6; _ij6[1] = -1;
6910 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
6914 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
6917 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
6920 IkReal x277=
IKcos(j6);
6921 IkReal x278=
IKsin(j6);
6922 IkReal x279=((1.0)*sj4);
6923 IkReal x280=((1.0)*x277);
6924 IkReal x281=((1.0)*x278);
6925 evalcond[0]=(x278+new_r21);
6926 evalcond[1]=((((-1.0)*x280))+new_r20);
6927 evalcond[2]=(new_r01+((sj4*x277)));
6928 evalcond[3]=(new_r00+((sj4*x278)));
6929 evalcond[4]=((((-1.0)*cj4*x280))+new_r11);
6930 evalcond[5]=((((-1.0)*cj4*x281))+new_r10);
6931 evalcond[6]=(((cj4*new_r10))+(((-1.0)*new_r00*x279))+(((-1.0)*x281)));
6932 evalcond[7]=((((-1.0)*new_r01*x279))+((cj4*new_r11))+(((-1.0)*x280)));
6940 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6941 vinfos[0].jointtype = 17;
6942 vinfos[0].foffset = j0;
6943 vinfos[0].indices[0] = _ij0[0];
6944 vinfos[0].indices[1] = _ij0[1];
6945 vinfos[0].maxsolutions = _nj0;
6946 vinfos[1].jointtype = 1;
6947 vinfos[1].foffset = j1;
6948 vinfos[1].indices[0] = _ij1[0];
6949 vinfos[1].indices[1] = _ij1[1];
6950 vinfos[1].maxsolutions = _nj1;
6951 vinfos[2].jointtype = 1;
6952 vinfos[2].foffset = j2;
6953 vinfos[2].indices[0] = _ij2[0];
6954 vinfos[2].indices[1] = _ij2[1];
6955 vinfos[2].maxsolutions = _nj2;
6956 vinfos[3].jointtype = 1;
6957 vinfos[3].foffset = j3;
6958 vinfos[3].indices[0] = _ij3[0];
6959 vinfos[3].indices[1] = _ij3[1];
6960 vinfos[3].maxsolutions = _nj3;
6961 vinfos[4].jointtype = 1;
6962 vinfos[4].foffset = j4;
6963 vinfos[4].indices[0] = _ij4[0];
6964 vinfos[4].indices[1] = _ij4[1];
6965 vinfos[4].maxsolutions = _nj4;
6966 vinfos[5].jointtype = 1;
6967 vinfos[5].foffset = j5;
6968 vinfos[5].indices[0] = _ij5[0];
6969 vinfos[5].indices[1] = _ij5[1];
6970 vinfos[5].maxsolutions = _nj5;
6971 vinfos[6].jointtype = 1;
6972 vinfos[6].foffset = j6;
6973 vinfos[6].indices[0] = _ij6[0];
6974 vinfos[6].indices[1] = _ij6[1];
6975 vinfos[6].maxsolutions = _nj6;
6976 std::vector<int> vfree(0);
6984 if( bgotonextstatement )
6986 bool bgotonextstatement =
true;
6989 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
6990 evalcond[1]=new_r20;
6991 evalcond[2]=new_r02;
6992 evalcond[3]=new_r12;
6993 evalcond[4]=new_r21;
6994 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
6996 bgotonextstatement=
false;
6998 IkReal j6array[1], cj6array[1], sj6array[1];
6999 bool j6valid[1]={
false};
7001 IkReal x282=((1.0)*new_r01);
7004 j6array[0]=
IKatan2(((((-1.0)*cj4*x282))+(((-1.0)*new_r00*sj4))), (((cj4*new_r00))+(((-1.0)*sj4*x282))));
7005 sj6array[0]=
IKsin(j6array[0]);
7006 cj6array[0]=
IKcos(j6array[0]);
7007 if( j6array[0] >
IKPI )
7011 else if( j6array[0] < -
IKPI )
7012 { j6array[0]+=
IK2PI;
7015 for(
int ij6 = 0; ij6 < 1; ++ij6)
7021 _ij6[0] = ij6; _ij6[1] = -1;
7022 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7026 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7029 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7032 IkReal x283=
IKsin(j6);
7033 IkReal x284=
IKcos(j6);
7034 IkReal x285=((1.0)*sj4);
7035 IkReal x286=((1.0)*x284);
7036 IkReal x287=(sj4*x283);
7037 IkReal x288=(sj4*x284);
7038 IkReal x289=(cj4*x283);
7039 IkReal x290=(cj4*x286);
7040 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x283);
7041 evalcond[1]=(x289+x288+new_r01);
7042 evalcond[2]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x286)));
7043 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x285))+(((-1.0)*x283)));
7044 evalcond[4]=(((cj4*new_r11))+(((-1.0)*new_r01*x285))+(((-1.0)*x286)));
7045 evalcond[5]=((((-1.0)*x290))+x287+new_r00);
7046 evalcond[6]=((((-1.0)*x290))+x287+new_r11);
7047 evalcond[7]=((((-1.0)*x289))+(((-1.0)*x284*x285))+new_r10);
7055 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7056 vinfos[0].jointtype = 17;
7057 vinfos[0].foffset = j0;
7058 vinfos[0].indices[0] = _ij0[0];
7059 vinfos[0].indices[1] = _ij0[1];
7060 vinfos[0].maxsolutions = _nj0;
7061 vinfos[1].jointtype = 1;
7062 vinfos[1].foffset = j1;
7063 vinfos[1].indices[0] = _ij1[0];
7064 vinfos[1].indices[1] = _ij1[1];
7065 vinfos[1].maxsolutions = _nj1;
7066 vinfos[2].jointtype = 1;
7067 vinfos[2].foffset = j2;
7068 vinfos[2].indices[0] = _ij2[0];
7069 vinfos[2].indices[1] = _ij2[1];
7070 vinfos[2].maxsolutions = _nj2;
7071 vinfos[3].jointtype = 1;
7072 vinfos[3].foffset = j3;
7073 vinfos[3].indices[0] = _ij3[0];
7074 vinfos[3].indices[1] = _ij3[1];
7075 vinfos[3].maxsolutions = _nj3;
7076 vinfos[4].jointtype = 1;
7077 vinfos[4].foffset = j4;
7078 vinfos[4].indices[0] = _ij4[0];
7079 vinfos[4].indices[1] = _ij4[1];
7080 vinfos[4].maxsolutions = _nj4;
7081 vinfos[5].jointtype = 1;
7082 vinfos[5].foffset = j5;
7083 vinfos[5].indices[0] = _ij5[0];
7084 vinfos[5].indices[1] = _ij5[1];
7085 vinfos[5].maxsolutions = _nj5;
7086 vinfos[6].jointtype = 1;
7087 vinfos[6].foffset = j6;
7088 vinfos[6].indices[0] = _ij6[0];
7089 vinfos[6].indices[1] = _ij6[1];
7090 vinfos[6].maxsolutions = _nj6;
7091 std::vector<int> vfree(0);
7099 if( bgotonextstatement )
7101 bool bgotonextstatement =
true;
7104 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
7105 evalcond[1]=new_r20;
7106 evalcond[2]=new_r02;
7107 evalcond[3]=new_r12;
7108 evalcond[4]=new_r21;
7109 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
7111 bgotonextstatement=
false;
7113 IkReal j6array[1], cj6array[1], sj6array[1];
7114 bool j6valid[1]={
false};
7116 IkReal x291=((1.0)*new_r00);
7119 j6array[0]=
IKatan2((((cj4*new_r01))+(((-1.0)*sj4*x291))), ((((-1.0)*new_r01*sj4))+(((-1.0)*cj4*x291))));
7120 sj6array[0]=
IKsin(j6array[0]);
7121 cj6array[0]=
IKcos(j6array[0]);
7122 if( j6array[0] >
IKPI )
7126 else if( j6array[0] < -
IKPI )
7127 { j6array[0]+=
IK2PI;
7130 for(
int ij6 = 0; ij6 < 1; ++ij6)
7136 _ij6[0] = ij6; _ij6[1] = -1;
7137 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7141 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7144 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7147 IkReal x292=
IKcos(j6);
7148 IkReal x293=
IKsin(j6);
7149 IkReal x294=((1.0)*sj4);
7150 IkReal x295=((1.0)*x293);
7151 IkReal x296=(sj4*x292);
7152 IkReal x297=((1.0)*x292);
7153 IkReal x298=(cj4*x295);
7154 evalcond[0]=(((cj4*new_r00))+((new_r10*sj4))+x292);
7155 evalcond[1]=(((cj4*new_r01))+((new_r11*sj4))+(((-1.0)*x295)));
7156 evalcond[2]=(((cj4*x292))+((sj4*x293))+new_r00);
7157 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x295))+(((-1.0)*new_r00*x294)));
7158 evalcond[4]=((((-1.0)*new_r01*x294))+((cj4*new_r11))+(((-1.0)*x297)));
7159 evalcond[5]=((((-1.0)*x298))+x296+new_r01);
7160 evalcond[6]=((((-1.0)*x298))+x296+new_r10);
7161 evalcond[7]=((((-1.0)*cj4*x297))+(((-1.0)*x293*x294))+new_r11);
7169 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7170 vinfos[0].jointtype = 17;
7171 vinfos[0].foffset = j0;
7172 vinfos[0].indices[0] = _ij0[0];
7173 vinfos[0].indices[1] = _ij0[1];
7174 vinfos[0].maxsolutions = _nj0;
7175 vinfos[1].jointtype = 1;
7176 vinfos[1].foffset = j1;
7177 vinfos[1].indices[0] = _ij1[0];
7178 vinfos[1].indices[1] = _ij1[1];
7179 vinfos[1].maxsolutions = _nj1;
7180 vinfos[2].jointtype = 1;
7181 vinfos[2].foffset = j2;
7182 vinfos[2].indices[0] = _ij2[0];
7183 vinfos[2].indices[1] = _ij2[1];
7184 vinfos[2].maxsolutions = _nj2;
7185 vinfos[3].jointtype = 1;
7186 vinfos[3].foffset = j3;
7187 vinfos[3].indices[0] = _ij3[0];
7188 vinfos[3].indices[1] = _ij3[1];
7189 vinfos[3].maxsolutions = _nj3;
7190 vinfos[4].jointtype = 1;
7191 vinfos[4].foffset = j4;
7192 vinfos[4].indices[0] = _ij4[0];
7193 vinfos[4].indices[1] = _ij4[1];
7194 vinfos[4].maxsolutions = _nj4;
7195 vinfos[5].jointtype = 1;
7196 vinfos[5].foffset = j5;
7197 vinfos[5].indices[0] = _ij5[0];
7198 vinfos[5].indices[1] = _ij5[1];
7199 vinfos[5].maxsolutions = _nj5;
7200 vinfos[6].jointtype = 1;
7201 vinfos[6].foffset = j6;
7202 vinfos[6].indices[0] = _ij6[0];
7203 vinfos[6].indices[1] = _ij6[1];
7204 vinfos[6].maxsolutions = _nj6;
7205 std::vector<int> vfree(0);
7213 if( bgotonextstatement )
7215 bool bgotonextstatement =
true;
7218 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
7219 evalcond[1]=new_r12;
7220 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
7222 bgotonextstatement=
false;
7224 IkReal j6array[1], cj6array[1], sj6array[1];
7225 bool j6valid[1]={
false};
7229 j6array[0]=
IKatan2(new_r10, new_r11);
7230 sj6array[0]=
IKsin(j6array[0]);
7231 cj6array[0]=
IKcos(j6array[0]);
7232 if( j6array[0] >
IKPI )
7236 else if( j6array[0] < -
IKPI )
7237 { j6array[0]+=
IK2PI;
7240 for(
int ij6 = 0; ij6 < 1; ++ij6)
7246 _ij6[0] = ij6; _ij6[1] = -1;
7247 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7251 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7254 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7257 IkReal x299=
IKcos(j6);
7258 IkReal x300=
IKsin(j6);
7259 IkReal x301=((1.0)*cj5);
7260 IkReal x302=((1.0)*x300);
7261 IkReal x303=((1.0)*x299);
7262 evalcond[0]=(((new_r02*x299))+new_r20);
7263 evalcond[1]=(new_r10+(((-1.0)*x302)));
7264 evalcond[2]=(new_r11+(((-1.0)*x303)));
7265 evalcond[3]=(new_r01+((cj5*x300)));
7266 evalcond[4]=((((-1.0)*new_r02*x302))+new_r21);
7267 evalcond[5]=((((-1.0)*x299*x301))+new_r00);
7268 evalcond[6]=(((new_r20*sj5))+x299+(((-1.0)*new_r00*x301)));
7269 evalcond[7]=((((-1.0)*new_r01*x301))+((new_r21*sj5))+(((-1.0)*x302)));
7277 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7278 vinfos[0].jointtype = 17;
7279 vinfos[0].foffset = j0;
7280 vinfos[0].indices[0] = _ij0[0];
7281 vinfos[0].indices[1] = _ij0[1];
7282 vinfos[0].maxsolutions = _nj0;
7283 vinfos[1].jointtype = 1;
7284 vinfos[1].foffset = j1;
7285 vinfos[1].indices[0] = _ij1[0];
7286 vinfos[1].indices[1] = _ij1[1];
7287 vinfos[1].maxsolutions = _nj1;
7288 vinfos[2].jointtype = 1;
7289 vinfos[2].foffset = j2;
7290 vinfos[2].indices[0] = _ij2[0];
7291 vinfos[2].indices[1] = _ij2[1];
7292 vinfos[2].maxsolutions = _nj2;
7293 vinfos[3].jointtype = 1;
7294 vinfos[3].foffset = j3;
7295 vinfos[3].indices[0] = _ij3[0];
7296 vinfos[3].indices[1] = _ij3[1];
7297 vinfos[3].maxsolutions = _nj3;
7298 vinfos[4].jointtype = 1;
7299 vinfos[4].foffset = j4;
7300 vinfos[4].indices[0] = _ij4[0];
7301 vinfos[4].indices[1] = _ij4[1];
7302 vinfos[4].maxsolutions = _nj4;
7303 vinfos[5].jointtype = 1;
7304 vinfos[5].foffset = j5;
7305 vinfos[5].indices[0] = _ij5[0];
7306 vinfos[5].indices[1] = _ij5[1];
7307 vinfos[5].maxsolutions = _nj5;
7308 vinfos[6].jointtype = 1;
7309 vinfos[6].foffset = j6;
7310 vinfos[6].indices[0] = _ij6[0];
7311 vinfos[6].indices[1] = _ij6[1];
7312 vinfos[6].maxsolutions = _nj6;
7313 std::vector<int> vfree(0);
7321 if( bgotonextstatement )
7323 bool bgotonextstatement =
true;
7326 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
7327 evalcond[1]=new_r12;
7328 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
7330 bgotonextstatement=
false;
7335 j4=3.14159265358979;
7337 j6eval[1]=
IKsign(new_r02);
7338 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
7339 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
7345 j4=3.14159265358979;
7347 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
7353 j4=3.14159265358979;
7356 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
7360 bool bgotonextstatement =
true;
7363 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
7364 evalcond[1]=new_r22;
7365 evalcond[2]=new_r01;
7366 evalcond[3]=new_r00;
7367 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
7369 bgotonextstatement=
false;
7371 IkReal j6array[1], cj6array[1], sj6array[1];
7372 bool j6valid[1]={
false};
7376 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
7377 sj6array[0]=
IKsin(j6array[0]);
7378 cj6array[0]=
IKcos(j6array[0]);
7379 if( j6array[0] >
IKPI )
7383 else if( j6array[0] < -
IKPI )
7384 { j6array[0]+=
IK2PI;
7387 for(
int ij6 = 0; ij6 < 1; ++ij6)
7393 _ij6[0] = ij6; _ij6[1] = -1;
7394 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7398 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7401 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7404 IkReal x304=
IKcos(j6);
7405 IkReal x305=((1.0)*(
IKsin(j6)));
7406 evalcond[0]=(x304+new_r20);
7407 evalcond[1]=(new_r21+(((-1.0)*x305)));
7408 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x305)));
7409 evalcond[3]=((((-1.0)*x304))+(((-1.0)*new_r11)));
7417 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7418 vinfos[0].jointtype = 17;
7419 vinfos[0].foffset = j0;
7420 vinfos[0].indices[0] = _ij0[0];
7421 vinfos[0].indices[1] = _ij0[1];
7422 vinfos[0].maxsolutions = _nj0;
7423 vinfos[1].jointtype = 1;
7424 vinfos[1].foffset = j1;
7425 vinfos[1].indices[0] = _ij1[0];
7426 vinfos[1].indices[1] = _ij1[1];
7427 vinfos[1].maxsolutions = _nj1;
7428 vinfos[2].jointtype = 1;
7429 vinfos[2].foffset = j2;
7430 vinfos[2].indices[0] = _ij2[0];
7431 vinfos[2].indices[1] = _ij2[1];
7432 vinfos[2].maxsolutions = _nj2;
7433 vinfos[3].jointtype = 1;
7434 vinfos[3].foffset = j3;
7435 vinfos[3].indices[0] = _ij3[0];
7436 vinfos[3].indices[1] = _ij3[1];
7437 vinfos[3].maxsolutions = _nj3;
7438 vinfos[4].jointtype = 1;
7439 vinfos[4].foffset = j4;
7440 vinfos[4].indices[0] = _ij4[0];
7441 vinfos[4].indices[1] = _ij4[1];
7442 vinfos[4].maxsolutions = _nj4;
7443 vinfos[5].jointtype = 1;
7444 vinfos[5].foffset = j5;
7445 vinfos[5].indices[0] = _ij5[0];
7446 vinfos[5].indices[1] = _ij5[1];
7447 vinfos[5].maxsolutions = _nj5;
7448 vinfos[6].jointtype = 1;
7449 vinfos[6].foffset = j6;
7450 vinfos[6].indices[0] = _ij6[0];
7451 vinfos[6].indices[1] = _ij6[1];
7452 vinfos[6].maxsolutions = _nj6;
7453 std::vector<int> vfree(0);
7461 if( bgotonextstatement )
7463 bool bgotonextstatement =
true;
7466 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
7467 evalcond[1]=new_r22;
7468 evalcond[2]=new_r01;
7469 evalcond[3]=new_r00;
7470 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
7472 bgotonextstatement=
false;
7474 IkReal j6array[1], cj6array[1], sj6array[1];
7475 bool j6valid[1]={
false};
7479 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
7480 sj6array[0]=
IKsin(j6array[0]);
7481 cj6array[0]=
IKcos(j6array[0]);
7482 if( j6array[0] >
IKPI )
7486 else if( j6array[0] < -
IKPI )
7487 { j6array[0]+=
IK2PI;
7490 for(
int ij6 = 0; ij6 < 1; ++ij6)
7496 _ij6[0] = ij6; _ij6[1] = -1;
7497 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7501 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7504 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7507 IkReal x306=
IKsin(j6);
7508 IkReal x307=((1.0)*(
IKcos(j6)));
7509 evalcond[0]=(x306+new_r21);
7510 evalcond[1]=(new_r20+(((-1.0)*x307)));
7511 evalcond[2]=((((-1.0)*x306))+(((-1.0)*new_r10)));
7512 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x307)));
7520 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7521 vinfos[0].jointtype = 17;
7522 vinfos[0].foffset = j0;
7523 vinfos[0].indices[0] = _ij0[0];
7524 vinfos[0].indices[1] = _ij0[1];
7525 vinfos[0].maxsolutions = _nj0;
7526 vinfos[1].jointtype = 1;
7527 vinfos[1].foffset = j1;
7528 vinfos[1].indices[0] = _ij1[0];
7529 vinfos[1].indices[1] = _ij1[1];
7530 vinfos[1].maxsolutions = _nj1;
7531 vinfos[2].jointtype = 1;
7532 vinfos[2].foffset = j2;
7533 vinfos[2].indices[0] = _ij2[0];
7534 vinfos[2].indices[1] = _ij2[1];
7535 vinfos[2].maxsolutions = _nj2;
7536 vinfos[3].jointtype = 1;
7537 vinfos[3].foffset = j3;
7538 vinfos[3].indices[0] = _ij3[0];
7539 vinfos[3].indices[1] = _ij3[1];
7540 vinfos[3].maxsolutions = _nj3;
7541 vinfos[4].jointtype = 1;
7542 vinfos[4].foffset = j4;
7543 vinfos[4].indices[0] = _ij4[0];
7544 vinfos[4].indices[1] = _ij4[1];
7545 vinfos[4].maxsolutions = _nj4;
7546 vinfos[5].jointtype = 1;
7547 vinfos[5].foffset = j5;
7548 vinfos[5].indices[0] = _ij5[0];
7549 vinfos[5].indices[1] = _ij5[1];
7550 vinfos[5].maxsolutions = _nj5;
7551 vinfos[6].jointtype = 1;
7552 vinfos[6].foffset = j6;
7553 vinfos[6].indices[0] = _ij6[0];
7554 vinfos[6].indices[1] = _ij6[1];
7555 vinfos[6].maxsolutions = _nj6;
7556 std::vector<int> vfree(0);
7564 if( bgotonextstatement )
7566 bool bgotonextstatement =
true;
7569 evalcond[0]=
IKabs(new_r02);
7570 evalcond[1]=new_r20;
7571 evalcond[2]=new_r21;
7572 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
7574 bgotonextstatement=
false;
7576 IkReal j6array[1], cj6array[1], sj6array[1];
7577 bool j6valid[1]={
false};
7581 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*cj5*new_r00));
7582 sj6array[0]=
IKsin(j6array[0]);
7583 cj6array[0]=
IKcos(j6array[0]);
7584 if( j6array[0] >
IKPI )
7588 else if( j6array[0] < -
IKPI )
7589 { j6array[0]+=
IK2PI;
7592 for(
int ij6 = 0; ij6 < 1; ++ij6)
7598 _ij6[0] = ij6; _ij6[1] = -1;
7599 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7603 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7606 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7609 IkReal x308=
IKcos(j6);
7610 IkReal x309=
IKsin(j6);
7611 IkReal x310=((1.0)*x309);
7612 IkReal x311=((1.0)*x308);
7613 evalcond[0]=(((cj5*new_r00))+x308);
7614 evalcond[1]=((((-1.0)*x310))+(((-1.0)*new_r10)));
7615 evalcond[2]=((((-1.0)*x311))+(((-1.0)*new_r11)));
7616 evalcond[3]=((((-1.0)*new_r01))+((cj5*x309)));
7617 evalcond[4]=(((cj5*new_r01))+(((-1.0)*x310)));
7618 evalcond[5]=((((-1.0)*cj5*x311))+(((-1.0)*new_r00)));
7626 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7627 vinfos[0].jointtype = 17;
7628 vinfos[0].foffset = j0;
7629 vinfos[0].indices[0] = _ij0[0];
7630 vinfos[0].indices[1] = _ij0[1];
7631 vinfos[0].maxsolutions = _nj0;
7632 vinfos[1].jointtype = 1;
7633 vinfos[1].foffset = j1;
7634 vinfos[1].indices[0] = _ij1[0];
7635 vinfos[1].indices[1] = _ij1[1];
7636 vinfos[1].maxsolutions = _nj1;
7637 vinfos[2].jointtype = 1;
7638 vinfos[2].foffset = j2;
7639 vinfos[2].indices[0] = _ij2[0];
7640 vinfos[2].indices[1] = _ij2[1];
7641 vinfos[2].maxsolutions = _nj2;
7642 vinfos[3].jointtype = 1;
7643 vinfos[3].foffset = j3;
7644 vinfos[3].indices[0] = _ij3[0];
7645 vinfos[3].indices[1] = _ij3[1];
7646 vinfos[3].maxsolutions = _nj3;
7647 vinfos[4].jointtype = 1;
7648 vinfos[4].foffset = j4;
7649 vinfos[4].indices[0] = _ij4[0];
7650 vinfos[4].indices[1] = _ij4[1];
7651 vinfos[4].maxsolutions = _nj4;
7652 vinfos[5].jointtype = 1;
7653 vinfos[5].foffset = j5;
7654 vinfos[5].indices[0] = _ij5[0];
7655 vinfos[5].indices[1] = _ij5[1];
7656 vinfos[5].maxsolutions = _nj5;
7657 vinfos[6].jointtype = 1;
7658 vinfos[6].foffset = j6;
7659 vinfos[6].indices[0] = _ij6[0];
7660 vinfos[6].indices[1] = _ij6[1];
7661 vinfos[6].maxsolutions = _nj6;
7662 std::vector<int> vfree(0);
7670 if( bgotonextstatement )
7672 bool bgotonextstatement =
true;
7675 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
7676 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
7678 bgotonextstatement=
false;
7680 IkReal j6array[1], cj6array[1], sj6array[1];
7681 bool j6valid[1]={
false};
7685 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
7686 sj6array[0]=
IKsin(j6array[0]);
7687 cj6array[0]=
IKcos(j6array[0]);
7688 if( j6array[0] >
IKPI )
7692 else if( j6array[0] < -
IKPI )
7693 { j6array[0]+=
IK2PI;
7696 for(
int ij6 = 0; ij6 < 1; ++ij6)
7702 _ij6[0] = ij6; _ij6[1] = -1;
7703 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7707 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7710 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7713 IkReal x312=
IKsin(j6);
7714 IkReal x313=
IKcos(j6);
7716 evalcond[1]=(new_r22*x312);
7717 evalcond[2]=((-1.0)*x312);
7718 evalcond[3]=((-1.0)*new_r22*x313);
7719 evalcond[4]=((((-1.0)*new_r10))+(((-1.0)*x312)));
7720 evalcond[5]=((((-1.0)*new_r11))+(((-1.0)*x313)));
7728 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7729 vinfos[0].jointtype = 17;
7730 vinfos[0].foffset = j0;
7731 vinfos[0].indices[0] = _ij0[0];
7732 vinfos[0].indices[1] = _ij0[1];
7733 vinfos[0].maxsolutions = _nj0;
7734 vinfos[1].jointtype = 1;
7735 vinfos[1].foffset = j1;
7736 vinfos[1].indices[0] = _ij1[0];
7737 vinfos[1].indices[1] = _ij1[1];
7738 vinfos[1].maxsolutions = _nj1;
7739 vinfos[2].jointtype = 1;
7740 vinfos[2].foffset = j2;
7741 vinfos[2].indices[0] = _ij2[0];
7742 vinfos[2].indices[1] = _ij2[1];
7743 vinfos[2].maxsolutions = _nj2;
7744 vinfos[3].jointtype = 1;
7745 vinfos[3].foffset = j3;
7746 vinfos[3].indices[0] = _ij3[0];
7747 vinfos[3].indices[1] = _ij3[1];
7748 vinfos[3].maxsolutions = _nj3;
7749 vinfos[4].jointtype = 1;
7750 vinfos[4].foffset = j4;
7751 vinfos[4].indices[0] = _ij4[0];
7752 vinfos[4].indices[1] = _ij4[1];
7753 vinfos[4].maxsolutions = _nj4;
7754 vinfos[5].jointtype = 1;
7755 vinfos[5].foffset = j5;
7756 vinfos[5].indices[0] = _ij5[0];
7757 vinfos[5].indices[1] = _ij5[1];
7758 vinfos[5].maxsolutions = _nj5;
7759 vinfos[6].jointtype = 1;
7760 vinfos[6].foffset = j6;
7761 vinfos[6].indices[0] = _ij6[0];
7762 vinfos[6].indices[1] = _ij6[1];
7763 vinfos[6].maxsolutions = _nj6;
7764 std::vector<int> vfree(0);
7772 if( bgotonextstatement )
7774 bool bgotonextstatement =
true;
7779 bgotonextstatement=
false;
7784 if( bgotonextstatement )
7796 IkReal j6array[1], cj6array[1], sj6array[1];
7797 bool j6valid[1]={
false};
7809 j6array[0]=
IKatan2(((-1.0)*new_r21*(x314.
value)), ((-1.0)*new_r00*(x315.
value)));
7810 sj6array[0]=
IKsin(j6array[0]);
7811 cj6array[0]=
IKcos(j6array[0]);
7812 if( j6array[0] >
IKPI )
7816 else if( j6array[0] < -
IKPI )
7817 { j6array[0]+=
IK2PI;
7820 for(
int ij6 = 0; ij6 < 1; ++ij6)
7826 _ij6[0] = ij6; _ij6[1] = -1;
7827 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7831 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7834 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7837 IkReal x316=
IKsin(j6);
7838 IkReal x317=
IKcos(j6);
7839 IkReal x318=((1.0)*x316);
7840 IkReal x319=((1.0)*x317);
7841 evalcond[0]=(((new_r02*x316))+new_r21);
7842 evalcond[1]=((((-1.0)*new_r02*x319))+new_r20);
7843 evalcond[2]=((((-1.0)*x318))+(((-1.0)*new_r10)));
7844 evalcond[3]=((((-1.0)*x319))+(((-1.0)*new_r11)));
7845 evalcond[4]=(((cj5*x316))+(((-1.0)*new_r01)));
7846 evalcond[5]=((((-1.0)*cj5*x319))+(((-1.0)*new_r00)));
7847 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+x317);
7848 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x318))+((new_r21*sj5)));
7856 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7857 vinfos[0].jointtype = 17;
7858 vinfos[0].foffset = j0;
7859 vinfos[0].indices[0] = _ij0[0];
7860 vinfos[0].indices[1] = _ij0[1];
7861 vinfos[0].maxsolutions = _nj0;
7862 vinfos[1].jointtype = 1;
7863 vinfos[1].foffset = j1;
7864 vinfos[1].indices[0] = _ij1[0];
7865 vinfos[1].indices[1] = _ij1[1];
7866 vinfos[1].maxsolutions = _nj1;
7867 vinfos[2].jointtype = 1;
7868 vinfos[2].foffset = j2;
7869 vinfos[2].indices[0] = _ij2[0];
7870 vinfos[2].indices[1] = _ij2[1];
7871 vinfos[2].maxsolutions = _nj2;
7872 vinfos[3].jointtype = 1;
7873 vinfos[3].foffset = j3;
7874 vinfos[3].indices[0] = _ij3[0];
7875 vinfos[3].indices[1] = _ij3[1];
7876 vinfos[3].maxsolutions = _nj3;
7877 vinfos[4].jointtype = 1;
7878 vinfos[4].foffset = j4;
7879 vinfos[4].indices[0] = _ij4[0];
7880 vinfos[4].indices[1] = _ij4[1];
7881 vinfos[4].maxsolutions = _nj4;
7882 vinfos[5].jointtype = 1;
7883 vinfos[5].foffset = j5;
7884 vinfos[5].indices[0] = _ij5[0];
7885 vinfos[5].indices[1] = _ij5[1];
7886 vinfos[5].maxsolutions = _nj5;
7887 vinfos[6].jointtype = 1;
7888 vinfos[6].foffset = j6;
7889 vinfos[6].indices[0] = _ij6[0];
7890 vinfos[6].indices[1] = _ij6[1];
7891 vinfos[6].maxsolutions = _nj6;
7892 std::vector<int> vfree(0);
7905 IkReal j6array[1], cj6array[1], sj6array[1];
7906 bool j6valid[1]={
false};
7914 j6array[0]=
IKatan2(((-1.0)*new_r10), (new_r20*(x320.
value)));
7915 sj6array[0]=
IKsin(j6array[0]);
7916 cj6array[0]=
IKcos(j6array[0]);
7917 if( j6array[0] >
IKPI )
7921 else if( j6array[0] < -
IKPI )
7922 { j6array[0]+=
IK2PI;
7925 for(
int ij6 = 0; ij6 < 1; ++ij6)
7931 _ij6[0] = ij6; _ij6[1] = -1;
7932 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
7936 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
7939 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
7942 IkReal x321=
IKsin(j6);
7943 IkReal x322=
IKcos(j6);
7944 IkReal x323=((1.0)*x321);
7945 IkReal x324=((1.0)*x322);
7946 evalcond[0]=(new_r21+((new_r02*x321)));
7947 evalcond[1]=((((-1.0)*new_r02*x324))+new_r20);
7948 evalcond[2]=((((-1.0)*x323))+(((-1.0)*new_r10)));
7949 evalcond[3]=((((-1.0)*x324))+(((-1.0)*new_r11)));
7950 evalcond[4]=(((cj5*x321))+(((-1.0)*new_r01)));
7951 evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj5*x324)));
7952 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+x322);
7953 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x323))+((new_r21*sj5)));
7961 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7962 vinfos[0].jointtype = 17;
7963 vinfos[0].foffset = j0;
7964 vinfos[0].indices[0] = _ij0[0];
7965 vinfos[0].indices[1] = _ij0[1];
7966 vinfos[0].maxsolutions = _nj0;
7967 vinfos[1].jointtype = 1;
7968 vinfos[1].foffset = j1;
7969 vinfos[1].indices[0] = _ij1[0];
7970 vinfos[1].indices[1] = _ij1[1];
7971 vinfos[1].maxsolutions = _nj1;
7972 vinfos[2].jointtype = 1;
7973 vinfos[2].foffset = j2;
7974 vinfos[2].indices[0] = _ij2[0];
7975 vinfos[2].indices[1] = _ij2[1];
7976 vinfos[2].maxsolutions = _nj2;
7977 vinfos[3].jointtype = 1;
7978 vinfos[3].foffset = j3;
7979 vinfos[3].indices[0] = _ij3[0];
7980 vinfos[3].indices[1] = _ij3[1];
7981 vinfos[3].maxsolutions = _nj3;
7982 vinfos[4].jointtype = 1;
7983 vinfos[4].foffset = j4;
7984 vinfos[4].indices[0] = _ij4[0];
7985 vinfos[4].indices[1] = _ij4[1];
7986 vinfos[4].maxsolutions = _nj4;
7987 vinfos[5].jointtype = 1;
7988 vinfos[5].foffset = j5;
7989 vinfos[5].indices[0] = _ij5[0];
7990 vinfos[5].indices[1] = _ij5[1];
7991 vinfos[5].maxsolutions = _nj5;
7992 vinfos[6].jointtype = 1;
7993 vinfos[6].foffset = j6;
7994 vinfos[6].indices[0] = _ij6[0];
7995 vinfos[6].indices[1] = _ij6[1];
7996 vinfos[6].maxsolutions = _nj6;
7997 std::vector<int> vfree(0);
8010 IkReal j6array[1], cj6array[1], sj6array[1];
8011 bool j6valid[1]={
false};
8021 j6array[0]=((-1.5707963267949)+(x325.
value)+(((1.5707963267949)*(x326.
value))));
8022 sj6array[0]=
IKsin(j6array[0]);
8023 cj6array[0]=
IKcos(j6array[0]);
8024 if( j6array[0] >
IKPI )
8028 else if( j6array[0] < -
IKPI )
8029 { j6array[0]+=
IK2PI;
8032 for(
int ij6 = 0; ij6 < 1; ++ij6)
8038 _ij6[0] = ij6; _ij6[1] = -1;
8039 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8043 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8046 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8049 IkReal x327=
IKsin(j6);
8050 IkReal x328=
IKcos(j6);
8051 IkReal x329=((1.0)*x327);
8052 IkReal x330=((1.0)*x328);
8053 evalcond[0]=(new_r21+((new_r02*x327)));
8054 evalcond[1]=(new_r20+(((-1.0)*new_r02*x330)));
8055 evalcond[2]=((((-1.0)*x329))+(((-1.0)*new_r10)));
8056 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x330)));
8057 evalcond[4]=(((cj5*x327))+(((-1.0)*new_r01)));
8058 evalcond[5]=((((-1.0)*cj5*x330))+(((-1.0)*new_r00)));
8059 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+x328);
8060 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x329))+((new_r21*sj5)));
8068 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8069 vinfos[0].jointtype = 17;
8070 vinfos[0].foffset = j0;
8071 vinfos[0].indices[0] = _ij0[0];
8072 vinfos[0].indices[1] = _ij0[1];
8073 vinfos[0].maxsolutions = _nj0;
8074 vinfos[1].jointtype = 1;
8075 vinfos[1].foffset = j1;
8076 vinfos[1].indices[0] = _ij1[0];
8077 vinfos[1].indices[1] = _ij1[1];
8078 vinfos[1].maxsolutions = _nj1;
8079 vinfos[2].jointtype = 1;
8080 vinfos[2].foffset = j2;
8081 vinfos[2].indices[0] = _ij2[0];
8082 vinfos[2].indices[1] = _ij2[1];
8083 vinfos[2].maxsolutions = _nj2;
8084 vinfos[3].jointtype = 1;
8085 vinfos[3].foffset = j3;
8086 vinfos[3].indices[0] = _ij3[0];
8087 vinfos[3].indices[1] = _ij3[1];
8088 vinfos[3].maxsolutions = _nj3;
8089 vinfos[4].jointtype = 1;
8090 vinfos[4].foffset = j4;
8091 vinfos[4].indices[0] = _ij4[0];
8092 vinfos[4].indices[1] = _ij4[1];
8093 vinfos[4].maxsolutions = _nj4;
8094 vinfos[5].jointtype = 1;
8095 vinfos[5].foffset = j5;
8096 vinfos[5].indices[0] = _ij5[0];
8097 vinfos[5].indices[1] = _ij5[1];
8098 vinfos[5].maxsolutions = _nj5;
8099 vinfos[6].jointtype = 1;
8100 vinfos[6].foffset = j6;
8101 vinfos[6].indices[0] = _ij6[0];
8102 vinfos[6].indices[1] = _ij6[1];
8103 vinfos[6].maxsolutions = _nj6;
8104 std::vector<int> vfree(0);
8116 if( bgotonextstatement )
8118 bool bgotonextstatement =
true;
8121 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
8122 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
8124 bgotonextstatement=
false;
8132 if(
IKabs(j6eval[0]) < 0.0000000100000000 )
8138 IkReal op[2+1], zeror[2];
8143 polyroots2(op,zeror,numroots);
8144 IkReal j6array[2], cj6array[2], sj6array[2], tempj6array[1];
8145 int numsolutions = 0;
8146 for(
int ij6 = 0; ij6 < numroots; ++ij6)
8148 IkReal htj6 = zeror[ij6];
8149 tempj6array[0]=((2.0)*(
atan(htj6)));
8150 for(
int kj6 = 0; kj6 < 1; ++kj6)
8152 j6array[numsolutions] = tempj6array[kj6];
8153 if( j6array[numsolutions] >
IKPI )
8155 j6array[numsolutions]-=
IK2PI;
8157 else if( j6array[numsolutions] < -
IKPI )
8159 j6array[numsolutions]+=
IK2PI;
8161 sj6array[numsolutions] =
IKsin(j6array[numsolutions]);
8162 cj6array[numsolutions] =
IKcos(j6array[numsolutions]);
8166 bool j6valid[2]={
true,
true};
8168 for(
int ij6 = 0; ij6 < numsolutions; ++ij6)
8174 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8177 _ij6[0] = ij6; _ij6[1] = -1;
8178 for(
int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
8182 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8186 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8187 vinfos[0].jointtype = 17;
8188 vinfos[0].foffset = j0;
8189 vinfos[0].indices[0] = _ij0[0];
8190 vinfos[0].indices[1] = _ij0[1];
8191 vinfos[0].maxsolutions = _nj0;
8192 vinfos[1].jointtype = 1;
8193 vinfos[1].foffset = j1;
8194 vinfos[1].indices[0] = _ij1[0];
8195 vinfos[1].indices[1] = _ij1[1];
8196 vinfos[1].maxsolutions = _nj1;
8197 vinfos[2].jointtype = 1;
8198 vinfos[2].foffset = j2;
8199 vinfos[2].indices[0] = _ij2[0];
8200 vinfos[2].indices[1] = _ij2[1];
8201 vinfos[2].maxsolutions = _nj2;
8202 vinfos[3].jointtype = 1;
8203 vinfos[3].foffset = j3;
8204 vinfos[3].indices[0] = _ij3[0];
8205 vinfos[3].indices[1] = _ij3[1];
8206 vinfos[3].maxsolutions = _nj3;
8207 vinfos[4].jointtype = 1;
8208 vinfos[4].foffset = j4;
8209 vinfos[4].indices[0] = _ij4[0];
8210 vinfos[4].indices[1] = _ij4[1];
8211 vinfos[4].maxsolutions = _nj4;
8212 vinfos[5].jointtype = 1;
8213 vinfos[5].foffset = j5;
8214 vinfos[5].indices[0] = _ij5[0];
8215 vinfos[5].indices[1] = _ij5[1];
8216 vinfos[5].maxsolutions = _nj5;
8217 vinfos[6].jointtype = 1;
8218 vinfos[6].foffset = j6;
8219 vinfos[6].indices[0] = _ij6[0];
8220 vinfos[6].indices[1] = _ij6[1];
8221 vinfos[6].maxsolutions = _nj6;
8222 std::vector<int> vfree(0);
8233 if( bgotonextstatement )
8235 bool bgotonextstatement =
true;
8240 bgotonextstatement=
false;
8245 if( bgotonextstatement )
8262 IkReal j6array[1], cj6array[1], sj6array[1];
8263 bool j6valid[1]={
false};
8269 IkReal x331=x332.
value;
8280 j6array[0]=
IKatan2((x331*(x333.
value)*(x334.
value)*((((new_r20*sj4))+(((-1.0)*new_r01*sj5))))), ((-1.0)*new_r20*x331));
8281 sj6array[0]=
IKsin(j6array[0]);
8282 cj6array[0]=
IKcos(j6array[0]);
8283 if( j6array[0] >
IKPI )
8287 else if( j6array[0] < -
IKPI )
8288 { j6array[0]+=
IK2PI;
8291 for(
int ij6 = 0; ij6 < 1; ++ij6)
8297 _ij6[0] = ij6; _ij6[1] = -1;
8298 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8302 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8305 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8307 IkReal evalcond[12];
8308 IkReal x335=
IKsin(j6);
8309 IkReal x336=
IKcos(j6);
8310 IkReal x337=((1.0)*sj4);
8311 IkReal x338=(cj4*new_r01);
8312 IkReal x339=((1.0)*cj5);
8313 IkReal x340=(cj4*new_r00);
8314 IkReal x341=((1.0)*x335);
8315 IkReal x342=(cj5*x335);
8316 IkReal x343=(cj4*x336);
8317 evalcond[0]=(((sj5*x336))+new_r20);
8318 evalcond[1]=((((-1.0)*sj5*x341))+new_r21);
8319 evalcond[2]=(((new_r11*sj4))+x338+x342);
8320 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x337))+(((-1.0)*x341)));
8321 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x336))+(((-1.0)*new_r01*x337)));
8322 evalcond[5]=(((cj4*x342))+((sj4*x336))+new_r01);
8323 evalcond[6]=(((new_r10*sj4))+x340+(((-1.0)*x336*x339)));
8324 evalcond[7]=(((sj4*x335))+new_r00+(((-1.0)*x339*x343)));
8325 evalcond[8]=(((sj4*x342))+new_r11+(((-1.0)*x343)));
8326 evalcond[9]=((((-1.0)*cj4*x341))+new_r10+(((-1.0)*cj5*x336*x337)));
8327 evalcond[10]=(((new_r20*sj5))+(((-1.0)*cj5*new_r10*x337))+x336+(((-1.0)*x339*x340)));
8328 evalcond[11]=((((-1.0)*x338*x339))+(((-1.0)*cj5*new_r11*x337))+((new_r21*sj5))+(((-1.0)*x341)));
8336 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8337 vinfos[0].jointtype = 17;
8338 vinfos[0].foffset = j0;
8339 vinfos[0].indices[0] = _ij0[0];
8340 vinfos[0].indices[1] = _ij0[1];
8341 vinfos[0].maxsolutions = _nj0;
8342 vinfos[1].jointtype = 1;
8343 vinfos[1].foffset = j1;
8344 vinfos[1].indices[0] = _ij1[0];
8345 vinfos[1].indices[1] = _ij1[1];
8346 vinfos[1].maxsolutions = _nj1;
8347 vinfos[2].jointtype = 1;
8348 vinfos[2].foffset = j2;
8349 vinfos[2].indices[0] = _ij2[0];
8350 vinfos[2].indices[1] = _ij2[1];
8351 vinfos[2].maxsolutions = _nj2;
8352 vinfos[3].jointtype = 1;
8353 vinfos[3].foffset = j3;
8354 vinfos[3].indices[0] = _ij3[0];
8355 vinfos[3].indices[1] = _ij3[1];
8356 vinfos[3].maxsolutions = _nj3;
8357 vinfos[4].jointtype = 1;
8358 vinfos[4].foffset = j4;
8359 vinfos[4].indices[0] = _ij4[0];
8360 vinfos[4].indices[1] = _ij4[1];
8361 vinfos[4].maxsolutions = _nj4;
8362 vinfos[5].jointtype = 1;
8363 vinfos[5].foffset = j5;
8364 vinfos[5].indices[0] = _ij5[0];
8365 vinfos[5].indices[1] = _ij5[1];
8366 vinfos[5].maxsolutions = _nj5;
8367 vinfos[6].jointtype = 1;
8368 vinfos[6].foffset = j6;
8369 vinfos[6].indices[0] = _ij6[0];
8370 vinfos[6].indices[1] = _ij6[1];
8371 vinfos[6].maxsolutions = _nj6;
8372 std::vector<int> vfree(0);
8385 IkReal j6array[1], cj6array[1], sj6array[1];
8386 bool j6valid[1]={
false};
8392 IkReal x344=x345.
value;
8399 j6array[0]=
IKatan2((x344*(x346.
value)*(((((-1.0)*new_r00*sj5))+(((-1.0)*cj4*cj5*new_r20))))), ((-1.0)*new_r20*x344));
8400 sj6array[0]=
IKsin(j6array[0]);
8401 cj6array[0]=
IKcos(j6array[0]);
8402 if( j6array[0] >
IKPI )
8406 else if( j6array[0] < -
IKPI )
8407 { j6array[0]+=
IK2PI;
8410 for(
int ij6 = 0; ij6 < 1; ++ij6)
8416 _ij6[0] = ij6; _ij6[1] = -1;
8417 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8421 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8424 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8426 IkReal evalcond[12];
8427 IkReal x347=
IKsin(j6);
8428 IkReal x348=
IKcos(j6);
8429 IkReal x349=((1.0)*sj4);
8430 IkReal x350=(cj4*new_r01);
8431 IkReal x351=((1.0)*cj5);
8432 IkReal x352=(cj4*new_r00);
8433 IkReal x353=((1.0)*x347);
8434 IkReal x354=(cj5*x347);
8435 IkReal x355=(cj4*x348);
8436 evalcond[0]=(((sj5*x348))+new_r20);
8437 evalcond[1]=(new_r21+(((-1.0)*sj5*x353)));
8438 evalcond[2]=(((new_r11*sj4))+x350+x354);
8439 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x353))+(((-1.0)*new_r00*x349)));
8440 evalcond[4]=(((cj4*new_r11))+(((-1.0)*new_r01*x349))+(((-1.0)*x348)));
8441 evalcond[5]=(((cj4*x354))+((sj4*x348))+new_r01);
8442 evalcond[6]=(((new_r10*sj4))+(((-1.0)*x348*x351))+x352);
8443 evalcond[7]=(((sj4*x347))+(((-1.0)*x351*x355))+new_r00);
8444 evalcond[8]=(((sj4*x354))+(((-1.0)*x355))+new_r11);
8445 evalcond[9]=((((-1.0)*cj4*x353))+new_r10+(((-1.0)*cj5*x348*x349)));
8446 evalcond[10]=(((new_r20*sj5))+(((-1.0)*cj5*new_r10*x349))+x348+(((-1.0)*x351*x352)));
8447 evalcond[11]=((((-1.0)*x353))+(((-1.0)*cj5*new_r11*x349))+((new_r21*sj5))+(((-1.0)*x350*x351)));
8455 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8456 vinfos[0].jointtype = 17;
8457 vinfos[0].foffset = j0;
8458 vinfos[0].indices[0] = _ij0[0];
8459 vinfos[0].indices[1] = _ij0[1];
8460 vinfos[0].maxsolutions = _nj0;
8461 vinfos[1].jointtype = 1;
8462 vinfos[1].foffset = j1;
8463 vinfos[1].indices[0] = _ij1[0];
8464 vinfos[1].indices[1] = _ij1[1];
8465 vinfos[1].maxsolutions = _nj1;
8466 vinfos[2].jointtype = 1;
8467 vinfos[2].foffset = j2;
8468 vinfos[2].indices[0] = _ij2[0];
8469 vinfos[2].indices[1] = _ij2[1];
8470 vinfos[2].maxsolutions = _nj2;
8471 vinfos[3].jointtype = 1;
8472 vinfos[3].foffset = j3;
8473 vinfos[3].indices[0] = _ij3[0];
8474 vinfos[3].indices[1] = _ij3[1];
8475 vinfos[3].maxsolutions = _nj3;
8476 vinfos[4].jointtype = 1;
8477 vinfos[4].foffset = j4;
8478 vinfos[4].indices[0] = _ij4[0];
8479 vinfos[4].indices[1] = _ij4[1];
8480 vinfos[4].maxsolutions = _nj4;
8481 vinfos[5].jointtype = 1;
8482 vinfos[5].foffset = j5;
8483 vinfos[5].indices[0] = _ij5[0];
8484 vinfos[5].indices[1] = _ij5[1];
8485 vinfos[5].maxsolutions = _nj5;
8486 vinfos[6].jointtype = 1;
8487 vinfos[6].foffset = j6;
8488 vinfos[6].indices[0] = _ij6[0];
8489 vinfos[6].indices[1] = _ij6[1];
8490 vinfos[6].maxsolutions = _nj6;
8491 std::vector<int> vfree(0);
8504 IkReal j6array[1], cj6array[1], sj6array[1];
8505 bool j6valid[1]={
false};
8515 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x356.
value)))+(x357.
value));
8516 sj6array[0]=
IKsin(j6array[0]);
8517 cj6array[0]=
IKcos(j6array[0]);
8518 if( j6array[0] >
IKPI )
8522 else if( j6array[0] < -
IKPI )
8523 { j6array[0]+=
IK2PI;
8526 for(
int ij6 = 0; ij6 < 1; ++ij6)
8532 _ij6[0] = ij6; _ij6[1] = -1;
8533 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8537 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8540 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8542 IkReal evalcond[12];
8543 IkReal x358=
IKsin(j6);
8544 IkReal x359=
IKcos(j6);
8545 IkReal x360=((1.0)*sj4);
8546 IkReal x361=(cj4*new_r01);
8547 IkReal x362=((1.0)*cj5);
8548 IkReal x363=(cj4*new_r00);
8549 IkReal x364=((1.0)*x358);
8550 IkReal x365=(cj5*x358);
8551 IkReal x366=(cj4*x359);
8552 evalcond[0]=(((sj5*x359))+new_r20);
8553 evalcond[1]=((((-1.0)*sj5*x364))+new_r21);
8554 evalcond[2]=(((new_r11*sj4))+x361+x365);
8555 evalcond[3]=((((-1.0)*new_r00*x360))+(((-1.0)*x364))+((cj4*new_r10)));
8556 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x359))+(((-1.0)*new_r01*x360)));
8557 evalcond[5]=(((sj4*x359))+((cj4*x365))+new_r01);
8558 evalcond[6]=(((new_r10*sj4))+x363+(((-1.0)*x359*x362)));
8559 evalcond[7]=(((sj4*x358))+(((-1.0)*x362*x366))+new_r00);
8560 evalcond[8]=((((-1.0)*x366))+((sj4*x365))+new_r11);
8561 evalcond[9]=((((-1.0)*cj4*x364))+(((-1.0)*cj5*x359*x360))+new_r10);
8562 evalcond[10]=((((-1.0)*x362*x363))+((new_r20*sj5))+x359+(((-1.0)*cj5*new_r10*x360)));
8563 evalcond[11]=((((-1.0)*x361*x362))+(((-1.0)*x364))+(((-1.0)*cj5*new_r11*x360))+((new_r21*sj5)));
8571 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8572 vinfos[0].jointtype = 17;
8573 vinfos[0].foffset = j0;
8574 vinfos[0].indices[0] = _ij0[0];
8575 vinfos[0].indices[1] = _ij0[1];
8576 vinfos[0].maxsolutions = _nj0;
8577 vinfos[1].jointtype = 1;
8578 vinfos[1].foffset = j1;
8579 vinfos[1].indices[0] = _ij1[0];
8580 vinfos[1].indices[1] = _ij1[1];
8581 vinfos[1].maxsolutions = _nj1;
8582 vinfos[2].jointtype = 1;
8583 vinfos[2].foffset = j2;
8584 vinfos[2].indices[0] = _ij2[0];
8585 vinfos[2].indices[1] = _ij2[1];
8586 vinfos[2].maxsolutions = _nj2;
8587 vinfos[3].jointtype = 1;
8588 vinfos[3].foffset = j3;
8589 vinfos[3].indices[0] = _ij3[0];
8590 vinfos[3].indices[1] = _ij3[1];
8591 vinfos[3].maxsolutions = _nj3;
8592 vinfos[4].jointtype = 1;
8593 vinfos[4].foffset = j4;
8594 vinfos[4].indices[0] = _ij4[0];
8595 vinfos[4].indices[1] = _ij4[1];
8596 vinfos[4].maxsolutions = _nj4;
8597 vinfos[5].jointtype = 1;
8598 vinfos[5].foffset = j5;
8599 vinfos[5].indices[0] = _ij5[0];
8600 vinfos[5].indices[1] = _ij5[1];
8601 vinfos[5].maxsolutions = _nj5;
8602 vinfos[6].jointtype = 1;
8603 vinfos[6].foffset = j6;
8604 vinfos[6].indices[0] = _ij6[0];
8605 vinfos[6].indices[1] = _ij6[1];
8606 vinfos[6].maxsolutions = _nj6;
8607 std::vector<int> vfree(0);
8626 IkReal j6array[1], cj6array[1], sj6array[1];
8627 bool j6valid[1]={
false};
8637 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x367.
value)))+(x368.
value));
8638 sj6array[0]=
IKsin(j6array[0]);
8639 cj6array[0]=
IKcos(j6array[0]);
8640 if( j6array[0] >
IKPI )
8644 else if( j6array[0] < -
IKPI )
8645 { j6array[0]+=
IK2PI;
8648 for(
int ij6 = 0; ij6 < 1; ++ij6)
8654 _ij6[0] = ij6; _ij6[1] = -1;
8655 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
8659 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
8662 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
8665 evalcond[0]=(new_r20+((sj5*(
IKcos(j6)))));
8666 evalcond[1]=((((-1.0)*sj5*(
IKsin(j6))))+new_r21);
8676 j4eval[1]=((
IKabs(new_r12))+(
IKabs(new_r02)));
8678 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 ||
IKabs(j4eval[2]) < 0.0000010000000000 )
8684 if(
IKabs(j4eval[0]) < 0.0000010000000000 ||
IKabs(j4eval[1]) < 0.0000010000000000 )
8688 bool bgotonextstatement =
true;
8691 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j6)))), 6.28318530717959)));
8692 evalcond[1]=new_r20;
8693 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8695 bgotonextstatement=
false;
8697 IkReal j4array[1], cj4array[1], sj4array[1];
8698 bool j4valid[1]={
false};
8702 j4array[0]=
IKatan2(((-1.0)*new_r00), new_r10);
8703 sj4array[0]=
IKsin(j4array[0]);
8704 cj4array[0]=
IKcos(j4array[0]);
8705 if( j4array[0] >
IKPI )
8709 else if( j4array[0] < -
IKPI )
8710 { j4array[0]+=
IK2PI;
8713 for(
int ij4 = 0; ij4 < 1; ++ij4)
8719 _ij4[0] = ij4; _ij4[1] = -1;
8720 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
8724 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
8727 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
8729 IkReal evalcond[18];
8730 IkReal x369=
IKsin(j4);
8731 IkReal x370=
IKcos(j4);
8732 IkReal x371=((1.0)*new_r12);
8733 IkReal x372=((1.0)*sj5);
8734 IkReal x373=((1.0)*new_r22);
8735 IkReal x374=(new_r11*x369);
8736 IkReal x375=(new_r22*x369);
8737 IkReal x376=(new_r02*x370);
8738 IkReal x377=((1.0)*x370);
8739 IkReal x378=(new_r10*x369);
8740 IkReal x379=((1.0)*x369);
8741 IkReal x380=(x370*x372);
8742 evalcond[0]=(x369+new_r00);
8743 evalcond[1]=(new_r01+((new_r22*x370)));
8744 evalcond[2]=(x375+new_r11);
8745 evalcond[3]=((((-1.0)*x377))+new_r10);
8746 evalcond[4]=((((-1.0)*x380))+new_r02);
8747 evalcond[5]=((((-1.0)*x369*x372))+new_r12);
8748 evalcond[6]=(x378+((new_r00*x370)));
8749 evalcond[7]=(((new_r12*x370))+(((-1.0)*new_r02*x379)));
8750 evalcond[8]=((((-1.0)*new_r01*x379))+((new_r11*x370)));
8751 evalcond[9]=(((new_r01*x370))+x374+new_r22);
8752 evalcond[10]=((-1.0)+(((-1.0)*new_r00*x379))+((new_r10*x370)));
8753 evalcond[11]=((((-1.0)*x372))+x376+((new_r12*x369)));
8754 evalcond[12]=((((-1.0)*new_r00*x380))+(((-1.0)*x372*x378)));
8755 evalcond[13]=((((-1.0)*x373*x378))+(((-1.0)*new_r00*x370*x373)));
8756 evalcond[14]=(((new_r22*sj5))+(((-1.0)*x373*x376))+(((-1.0)*x371*x375)));
8757 evalcond[15]=((((-1.0)*cj5*new_r21))+(((-1.0)*x372*x374))+(((-1.0)*new_r01*x380)));
8758 evalcond[16]=((-1.0)+(((-1.0)*x373*x374))+(((-1.0)*new_r01*x370*x373))+(sj5*sj5));
8759 evalcond[17]=((1.0)+(((-1.0)*x372*x376))+(((-1.0)*new_r22*x373))+(((-1.0)*sj5*x369*x371)));
8767 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8768 vinfos[0].jointtype = 17;
8769 vinfos[0].foffset = j0;
8770 vinfos[0].indices[0] = _ij0[0];
8771 vinfos[0].indices[1] = _ij0[1];
8772 vinfos[0].maxsolutions = _nj0;
8773 vinfos[1].jointtype = 1;
8774 vinfos[1].foffset = j1;
8775 vinfos[1].indices[0] = _ij1[0];
8776 vinfos[1].indices[1] = _ij1[1];
8777 vinfos[1].maxsolutions = _nj1;
8778 vinfos[2].jointtype = 1;
8779 vinfos[2].foffset = j2;
8780 vinfos[2].indices[0] = _ij2[0];
8781 vinfos[2].indices[1] = _ij2[1];
8782 vinfos[2].maxsolutions = _nj2;
8783 vinfos[3].jointtype = 1;
8784 vinfos[3].foffset = j3;
8785 vinfos[3].indices[0] = _ij3[0];
8786 vinfos[3].indices[1] = _ij3[1];
8787 vinfos[3].maxsolutions = _nj3;
8788 vinfos[4].jointtype = 1;
8789 vinfos[4].foffset = j4;
8790 vinfos[4].indices[0] = _ij4[0];
8791 vinfos[4].indices[1] = _ij4[1];
8792 vinfos[4].maxsolutions = _nj4;
8793 vinfos[5].jointtype = 1;
8794 vinfos[5].foffset = j5;
8795 vinfos[5].indices[0] = _ij5[0];
8796 vinfos[5].indices[1] = _ij5[1];
8797 vinfos[5].maxsolutions = _nj5;
8798 vinfos[6].jointtype = 1;
8799 vinfos[6].foffset = j6;
8800 vinfos[6].indices[0] = _ij6[0];
8801 vinfos[6].indices[1] = _ij6[1];
8802 vinfos[6].maxsolutions = _nj6;
8803 std::vector<int> vfree(0);
8811 if( bgotonextstatement )
8813 bool bgotonextstatement =
true;
8816 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j6)))), 6.28318530717959)));
8817 evalcond[1]=new_r20;
8818 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
8820 bgotonextstatement=
false;
8822 IkReal j4array[1], cj4array[1], sj4array[1];
8823 bool j4valid[1]={
false};
8827 j4array[0]=
IKatan2(new_r00, ((-1.0)*new_r10));
8828 sj4array[0]=
IKsin(j4array[0]);
8829 cj4array[0]=
IKcos(j4array[0]);
8830 if( j4array[0] >
IKPI )
8834 else if( j4array[0] < -
IKPI )
8835 { j4array[0]+=
IK2PI;
8838 for(
int ij4 = 0; ij4 < 1; ++ij4)
8844 _ij4[0] = ij4; _ij4[1] = -1;
8845 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
8849 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
8852 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
8854 IkReal evalcond[18];
8855 IkReal x381=
IKcos(j4);
8856 IkReal x382=
IKsin(j4);
8857 IkReal x383=(new_r22*sj5);
8858 IkReal x384=((1.0)*new_r22);
8859 IkReal x385=((1.0)*sj5);
8860 IkReal x386=((1.0)*x381);
8861 IkReal x387=(new_r11*x382);
8862 IkReal x388=(new_r12*x382);
8863 IkReal x389=((1.0)*x382);
8864 IkReal x390=(new_r10*x382);
8865 evalcond[0]=(x381+new_r10);
8866 evalcond[1]=((((-1.0)*x389))+new_r00);
8867 evalcond[2]=((((-1.0)*x381*x385))+new_r02);
8868 evalcond[3]=(new_r12+(((-1.0)*x382*x385)));
8869 evalcond[4]=((((-1.0)*x381*x384))+new_r01);
8870 evalcond[5]=(new_r11+(((-1.0)*x382*x384)));
8871 evalcond[6]=(((new_r00*x381))+x390);
8872 evalcond[7]=(((new_r12*x381))+(((-1.0)*new_r02*x389)));
8873 evalcond[8]=((((-1.0)*new_r01*x389))+((new_r11*x381)));
8874 evalcond[9]=((1.0)+(((-1.0)*new_r00*x389))+((new_r10*x381)));
8875 evalcond[10]=(((new_r02*x381))+x388+(((-1.0)*x385)));
8876 evalcond[11]=(((new_r01*x381))+x387+(((-1.0)*x384)));
8877 evalcond[12]=((((-1.0)*new_r00*x381*x385))+(((-1.0)*x385*x390)));
8878 evalcond[13]=((((-1.0)*x384*x390))+(((-1.0)*new_r00*x381*x384)));
8879 evalcond[14]=((((-1.0)*new_r02*x381*x384))+(((-1.0)*x384*x388))+x383);
8880 evalcond[15]=(x383+(((-1.0)*new_r01*x381*x385))+(((-1.0)*x385*x387)));
8881 evalcond[16]=((1.0)+(((-1.0)*new_r02*x381*x385))+(((-1.0)*new_r22*x384))+(((-1.0)*x385*x388)));
8882 evalcond[17]=((1.0)+(((-1.0)*sj5*x385))+(((-1.0)*x384*x387))+(((-1.0)*new_r01*x381*x384)));
8890 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8891 vinfos[0].jointtype = 17;
8892 vinfos[0].foffset = j0;
8893 vinfos[0].indices[0] = _ij0[0];
8894 vinfos[0].indices[1] = _ij0[1];
8895 vinfos[0].maxsolutions = _nj0;
8896 vinfos[1].jointtype = 1;
8897 vinfos[1].foffset = j1;
8898 vinfos[1].indices[0] = _ij1[0];
8899 vinfos[1].indices[1] = _ij1[1];
8900 vinfos[1].maxsolutions = _nj1;
8901 vinfos[2].jointtype = 1;
8902 vinfos[2].foffset = j2;
8903 vinfos[2].indices[0] = _ij2[0];
8904 vinfos[2].indices[1] = _ij2[1];
8905 vinfos[2].maxsolutions = _nj2;
8906 vinfos[3].jointtype = 1;
8907 vinfos[3].foffset = j3;
8908 vinfos[3].indices[0] = _ij3[0];
8909 vinfos[3].indices[1] = _ij3[1];
8910 vinfos[3].maxsolutions = _nj3;
8911 vinfos[4].jointtype = 1;
8912 vinfos[4].foffset = j4;
8913 vinfos[4].indices[0] = _ij4[0];
8914 vinfos[4].indices[1] = _ij4[1];
8915 vinfos[4].maxsolutions = _nj4;
8916 vinfos[5].jointtype = 1;
8917 vinfos[5].foffset = j5;
8918 vinfos[5].indices[0] = _ij5[0];
8919 vinfos[5].indices[1] = _ij5[1];
8920 vinfos[5].maxsolutions = _nj5;
8921 vinfos[6].jointtype = 1;
8922 vinfos[6].foffset = j6;
8923 vinfos[6].indices[0] = _ij6[0];
8924 vinfos[6].indices[1] = _ij6[1];
8925 vinfos[6].maxsolutions = _nj6;
8926 std::vector<int> vfree(0);
8934 if( bgotonextstatement )
8936 bool bgotonextstatement =
true;
8939 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
8940 evalcond[1]=new_r20;
8941 evalcond[2]=new_r02;
8942 evalcond[3]=new_r12;
8943 evalcond[4]=new_r21;
8944 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
8946 bgotonextstatement=
false;
8948 IkReal j4array[1], cj4array[1], sj4array[1];
8949 bool j4valid[1]={
false};
8951 IkReal x391=((1.0)*sj6);
8954 j4array[0]=
IKatan2(((((-1.0)*cj6*new_r01))+(((-1.0)*new_r00*x391))), ((((-1.0)*new_r01*x391))+((cj6*new_r00))));
8955 sj4array[0]=
IKsin(j4array[0]);
8956 cj4array[0]=
IKcos(j4array[0]);
8957 if( j4array[0] >
IKPI )
8961 else if( j4array[0] < -
IKPI )
8962 { j4array[0]+=
IK2PI;
8965 for(
int ij4 = 0; ij4 < 1; ++ij4)
8971 _ij4[0] = ij4; _ij4[1] = -1;
8972 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
8976 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
8979 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
8982 IkReal x392=
IKcos(j4);
8983 IkReal x393=
IKsin(j4);
8984 IkReal x394=((1.0)*cj6);
8985 IkReal x395=(sj6*x393);
8986 IkReal x396=(cj6*x393);
8987 IkReal x397=(sj6*x392);
8988 IkReal x398=((1.0)*x393);
8989 IkReal x399=(x392*x394);
8990 evalcond[0]=(((new_r11*x393))+sj6+((new_r01*x392)));
8991 evalcond[1]=(x397+x396+new_r01);
8992 evalcond[2]=((((-1.0)*x399))+x395+new_r00);
8993 evalcond[3]=((((-1.0)*x399))+x395+new_r11);
8994 evalcond[4]=((((-1.0)*x394))+((new_r00*x392))+((new_r10*x393)));
8995 evalcond[5]=((((-1.0)*x397))+new_r10+(((-1.0)*x393*x394)));
8996 evalcond[6]=((((-1.0)*sj6))+(((-1.0)*new_r00*x398))+((new_r10*x392)));
8997 evalcond[7]=((((-1.0)*new_r01*x398))+((new_r11*x392))+(((-1.0)*x394)));
9005 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9006 vinfos[0].jointtype = 17;
9007 vinfos[0].foffset = j0;
9008 vinfos[0].indices[0] = _ij0[0];
9009 vinfos[0].indices[1] = _ij0[1];
9010 vinfos[0].maxsolutions = _nj0;
9011 vinfos[1].jointtype = 1;
9012 vinfos[1].foffset = j1;
9013 vinfos[1].indices[0] = _ij1[0];
9014 vinfos[1].indices[1] = _ij1[1];
9015 vinfos[1].maxsolutions = _nj1;
9016 vinfos[2].jointtype = 1;
9017 vinfos[2].foffset = j2;
9018 vinfos[2].indices[0] = _ij2[0];
9019 vinfos[2].indices[1] = _ij2[1];
9020 vinfos[2].maxsolutions = _nj2;
9021 vinfos[3].jointtype = 1;
9022 vinfos[3].foffset = j3;
9023 vinfos[3].indices[0] = _ij3[0];
9024 vinfos[3].indices[1] = _ij3[1];
9025 vinfos[3].maxsolutions = _nj3;
9026 vinfos[4].jointtype = 1;
9027 vinfos[4].foffset = j4;
9028 vinfos[4].indices[0] = _ij4[0];
9029 vinfos[4].indices[1] = _ij4[1];
9030 vinfos[4].maxsolutions = _nj4;
9031 vinfos[5].jointtype = 1;
9032 vinfos[5].foffset = j5;
9033 vinfos[5].indices[0] = _ij5[0];
9034 vinfos[5].indices[1] = _ij5[1];
9035 vinfos[5].maxsolutions = _nj5;
9036 vinfos[6].jointtype = 1;
9037 vinfos[6].foffset = j6;
9038 vinfos[6].indices[0] = _ij6[0];
9039 vinfos[6].indices[1] = _ij6[1];
9040 vinfos[6].maxsolutions = _nj6;
9041 std::vector<int> vfree(0);
9049 if( bgotonextstatement )
9051 bool bgotonextstatement =
true;
9054 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
9055 evalcond[1]=new_r20;
9056 evalcond[2]=new_r02;
9057 evalcond[3]=new_r12;
9058 evalcond[4]=new_r21;
9059 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
9061 bgotonextstatement=
false;
9063 IkReal j4array[1], cj4array[1], sj4array[1];
9064 bool j4valid[1]={
false};
9066 IkReal x400=((1.0)*new_r00);
9069 j4array[0]=
IKatan2(((((-1.0)*cj6*new_r01))+(((-1.0)*sj6*x400))), ((((-1.0)*cj6*x400))+((new_r01*sj6))));
9070 sj4array[0]=
IKsin(j4array[0]);
9071 cj4array[0]=
IKcos(j4array[0]);
9072 if( j4array[0] >
IKPI )
9076 else if( j4array[0] < -
IKPI )
9077 { j4array[0]+=
IK2PI;
9080 for(
int ij4 = 0; ij4 < 1; ++ij4)
9086 _ij4[0] = ij4; _ij4[1] = -1;
9087 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
9091 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9094 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9097 IkReal x401=
IKsin(j4);
9098 IkReal x402=
IKcos(j4);
9099 IkReal x403=((1.0)*sj6);
9100 IkReal x404=(cj6*x401);
9101 IkReal x405=(cj6*x402);
9102 IkReal x406=((1.0)*x401);
9103 IkReal x407=(x402*x403);
9104 evalcond[0]=(cj6+((new_r10*x401))+((new_r00*x402)));
9105 evalcond[1]=(((sj6*x401))+x405+new_r00);
9106 evalcond[2]=(x404+new_r01+(((-1.0)*x407)));
9107 evalcond[3]=(x404+new_r10+(((-1.0)*x407)));
9108 evalcond[4]=(((new_r11*x401))+((new_r01*x402))+(((-1.0)*x403)));
9109 evalcond[5]=((((-1.0)*x401*x403))+new_r11+(((-1.0)*x405)));
9110 evalcond[6]=(((new_r10*x402))+(((-1.0)*new_r00*x406))+(((-1.0)*x403)));
9111 evalcond[7]=(((new_r11*x402))+(((-1.0)*new_r01*x406))+(((-1.0)*cj6)));
9119 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9120 vinfos[0].jointtype = 17;
9121 vinfos[0].foffset = j0;
9122 vinfos[0].indices[0] = _ij0[0];
9123 vinfos[0].indices[1] = _ij0[1];
9124 vinfos[0].maxsolutions = _nj0;
9125 vinfos[1].jointtype = 1;
9126 vinfos[1].foffset = j1;
9127 vinfos[1].indices[0] = _ij1[0];
9128 vinfos[1].indices[1] = _ij1[1];
9129 vinfos[1].maxsolutions = _nj1;
9130 vinfos[2].jointtype = 1;
9131 vinfos[2].foffset = j2;
9132 vinfos[2].indices[0] = _ij2[0];
9133 vinfos[2].indices[1] = _ij2[1];
9134 vinfos[2].maxsolutions = _nj2;
9135 vinfos[3].jointtype = 1;
9136 vinfos[3].foffset = j3;
9137 vinfos[3].indices[0] = _ij3[0];
9138 vinfos[3].indices[1] = _ij3[1];
9139 vinfos[3].maxsolutions = _nj3;
9140 vinfos[4].jointtype = 1;
9141 vinfos[4].foffset = j4;
9142 vinfos[4].indices[0] = _ij4[0];
9143 vinfos[4].indices[1] = _ij4[1];
9144 vinfos[4].maxsolutions = _nj4;
9145 vinfos[5].jointtype = 1;
9146 vinfos[5].foffset = j5;
9147 vinfos[5].indices[0] = _ij5[0];
9148 vinfos[5].indices[1] = _ij5[1];
9149 vinfos[5].maxsolutions = _nj5;
9150 vinfos[6].jointtype = 1;
9151 vinfos[6].foffset = j6;
9152 vinfos[6].indices[0] = _ij6[0];
9153 vinfos[6].indices[1] = _ij6[1];
9154 vinfos[6].maxsolutions = _nj6;
9155 std::vector<int> vfree(0);
9163 if( bgotonextstatement )
9165 bool bgotonextstatement =
true;
9168 evalcond[0]=((
IKabs(new_r12))+(
IKabs(new_r02)));
9169 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
9171 bgotonextstatement=
false;
9178 j4eval[0]=((
IKabs(new_r11))+(
IKabs(new_r01)));
9179 if(
IKabs(j4eval[0]) < 0.0000010000000000 )
9187 j4eval[0]=((
IKabs(new_r10))+(
IKabs(new_r00)));
9188 if(
IKabs(j4eval[0]) < 0.0000010000000000 )
9196 j4eval[0]=((
IKabs((new_r10*new_r22)))+(
IKabs((new_r00*new_r22))));
9197 if(
IKabs(j4eval[0]) < 0.0000010000000000 )
9204 IkReal j4array[2], cj4array[2], sj4array[2];
9205 bool j4valid[2]={
false};
9207 IkReal x408=((-1.0)*new_r22);
9212 IkReal x409=x410.
value;
9213 j4array[0]=((-1.0)*x409);
9214 sj4array[0]=
IKsin(j4array[0]);
9215 cj4array[0]=
IKcos(j4array[0]);
9216 j4array[1]=((3.14159265358979)+(((-1.0)*x409)));
9217 sj4array[1]=
IKsin(j4array[1]);
9218 cj4array[1]=
IKcos(j4array[1]);
9219 if( j4array[0] >
IKPI )
9223 else if( j4array[0] < -
IKPI )
9224 { j4array[0]+=
IK2PI;
9227 if( j4array[1] >
IKPI )
9231 else if( j4array[1] < -
IKPI )
9232 { j4array[1]+=
IK2PI;
9235 for(
int ij4 = 0; ij4 < 2; ++ij4)
9241 _ij4[0] = ij4; _ij4[1] = -1;
9242 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
9246 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9249 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9252 IkReal x411=
IKcos(j4);
9253 IkReal x412=
IKsin(j4);
9254 IkReal x413=((1.0)*new_r22);
9255 IkReal x414=(new_r01*x411);
9256 IkReal x415=(new_r11*x412);
9257 IkReal x416=((1.0)*x412);
9258 evalcond[0]=(x414+x415);
9259 evalcond[1]=(((new_r10*x412))+((new_r00*x411)));
9260 evalcond[2]=(((new_r10*x411))+(((-1.0)*new_r00*x416)));
9261 evalcond[3]=(((new_r11*x411))+(((-1.0)*new_r01*x416)));
9262 evalcond[4]=((((-1.0)*x413*x414))+(((-1.0)*x413*x415)));
9270 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9271 vinfos[0].jointtype = 17;
9272 vinfos[0].foffset = j0;
9273 vinfos[0].indices[0] = _ij0[0];
9274 vinfos[0].indices[1] = _ij0[1];
9275 vinfos[0].maxsolutions = _nj0;
9276 vinfos[1].jointtype = 1;
9277 vinfos[1].foffset = j1;
9278 vinfos[1].indices[0] = _ij1[0];
9279 vinfos[1].indices[1] = _ij1[1];
9280 vinfos[1].maxsolutions = _nj1;
9281 vinfos[2].jointtype = 1;
9282 vinfos[2].foffset = j2;
9283 vinfos[2].indices[0] = _ij2[0];
9284 vinfos[2].indices[1] = _ij2[1];
9285 vinfos[2].maxsolutions = _nj2;
9286 vinfos[3].jointtype = 1;
9287 vinfos[3].foffset = j3;
9288 vinfos[3].indices[0] = _ij3[0];
9289 vinfos[3].indices[1] = _ij3[1];
9290 vinfos[3].maxsolutions = _nj3;
9291 vinfos[4].jointtype = 1;
9292 vinfos[4].foffset = j4;
9293 vinfos[4].indices[0] = _ij4[0];
9294 vinfos[4].indices[1] = _ij4[1];
9295 vinfos[4].maxsolutions = _nj4;
9296 vinfos[5].jointtype = 1;
9297 vinfos[5].foffset = j5;
9298 vinfos[5].indices[0] = _ij5[0];
9299 vinfos[5].indices[1] = _ij5[1];
9300 vinfos[5].maxsolutions = _nj5;
9301 vinfos[6].jointtype = 1;
9302 vinfos[6].foffset = j6;
9303 vinfos[6].indices[0] = _ij6[0];
9304 vinfos[6].indices[1] = _ij6[1];
9305 vinfos[6].maxsolutions = _nj6;
9306 std::vector<int> vfree(0);
9319 IkReal j4array[2], cj4array[2], sj4array[2];
9320 bool j4valid[2]={
false};
9326 IkReal x417=x418.
value;
9327 j4array[0]=((-1.0)*x417);
9328 sj4array[0]=
IKsin(j4array[0]);
9329 cj4array[0]=
IKcos(j4array[0]);
9330 j4array[1]=((3.14159265358979)+(((-1.0)*x417)));
9331 sj4array[1]=
IKsin(j4array[1]);
9332 cj4array[1]=
IKcos(j4array[1]);
9333 if( j4array[0] >
IKPI )
9337 else if( j4array[0] < -
IKPI )
9338 { j4array[0]+=
IK2PI;
9341 if( j4array[1] >
IKPI )
9345 else if( j4array[1] < -
IKPI )
9346 { j4array[1]+=
IK2PI;
9349 for(
int ij4 = 0; ij4 < 2; ++ij4)
9355 _ij4[0] = ij4; _ij4[1] = -1;
9356 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
9360 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9363 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9366 IkReal x419=
IKcos(j4);
9367 IkReal x420=
IKsin(j4);
9368 IkReal x421=((1.0)*new_r22);
9369 IkReal x422=(new_r01*x419);
9370 IkReal x423=(new_r11*x420);
9371 IkReal x424=((1.0)*x420);
9372 evalcond[0]=(x423+x422);
9373 evalcond[1]=((((-1.0)*new_r00*x424))+((new_r10*x419)));
9374 evalcond[2]=(((new_r11*x419))+(((-1.0)*new_r01*x424)));
9375 evalcond[3]=((((-1.0)*new_r10*x420*x421))+(((-1.0)*new_r00*x419*x421)));
9376 evalcond[4]=((((-1.0)*x421*x422))+(((-1.0)*x421*x423)));
9384 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9385 vinfos[0].jointtype = 17;
9386 vinfos[0].foffset = j0;
9387 vinfos[0].indices[0] = _ij0[0];
9388 vinfos[0].indices[1] = _ij0[1];
9389 vinfos[0].maxsolutions = _nj0;
9390 vinfos[1].jointtype = 1;
9391 vinfos[1].foffset = j1;
9392 vinfos[1].indices[0] = _ij1[0];
9393 vinfos[1].indices[1] = _ij1[1];
9394 vinfos[1].maxsolutions = _nj1;
9395 vinfos[2].jointtype = 1;
9396 vinfos[2].foffset = j2;
9397 vinfos[2].indices[0] = _ij2[0];
9398 vinfos[2].indices[1] = _ij2[1];
9399 vinfos[2].maxsolutions = _nj2;
9400 vinfos[3].jointtype = 1;
9401 vinfos[3].foffset = j3;
9402 vinfos[3].indices[0] = _ij3[0];
9403 vinfos[3].indices[1] = _ij3[1];
9404 vinfos[3].maxsolutions = _nj3;
9405 vinfos[4].jointtype = 1;
9406 vinfos[4].foffset = j4;
9407 vinfos[4].indices[0] = _ij4[0];
9408 vinfos[4].indices[1] = _ij4[1];
9409 vinfos[4].maxsolutions = _nj4;
9410 vinfos[5].jointtype = 1;
9411 vinfos[5].foffset = j5;
9412 vinfos[5].indices[0] = _ij5[0];
9413 vinfos[5].indices[1] = _ij5[1];
9414 vinfos[5].maxsolutions = _nj5;
9415 vinfos[6].jointtype = 1;
9416 vinfos[6].foffset = j6;
9417 vinfos[6].indices[0] = _ij6[0];
9418 vinfos[6].indices[1] = _ij6[1];
9419 vinfos[6].maxsolutions = _nj6;
9420 std::vector<int> vfree(0);
9433 IkReal j4array[2], cj4array[2], sj4array[2];
9434 bool j4valid[2]={
false};
9440 IkReal x425=x426.
value;
9441 j4array[0]=((-1.0)*x425);
9442 sj4array[0]=
IKsin(j4array[0]);
9443 cj4array[0]=
IKcos(j4array[0]);
9444 j4array[1]=((3.14159265358979)+(((-1.0)*x425)));
9445 sj4array[1]=
IKsin(j4array[1]);
9446 cj4array[1]=
IKcos(j4array[1]);
9447 if( j4array[0] >
IKPI )
9451 else if( j4array[0] < -
IKPI )
9452 { j4array[0]+=
IK2PI;
9455 if( j4array[1] >
IKPI )
9459 else if( j4array[1] < -
IKPI )
9460 { j4array[1]+=
IK2PI;
9463 for(
int ij4 = 0; ij4 < 2; ++ij4)
9469 _ij4[0] = ij4; _ij4[1] = -1;
9470 for(
int iij4 = ij4+1; iij4 < 2; ++iij4)
9474 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9477 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9480 IkReal x427=
IKcos(j4);
9481 IkReal x428=
IKsin(j4);
9482 IkReal x429=((1.0)*new_r22);
9483 IkReal x430=(new_r00*x427);
9484 IkReal x431=(new_r10*x428);
9485 IkReal x432=((1.0)*x428);
9486 evalcond[0]=(x430+x431);
9487 evalcond[1]=((((-1.0)*new_r00*x432))+((new_r10*x427)));
9488 evalcond[2]=((((-1.0)*new_r01*x432))+((new_r11*x427)));
9489 evalcond[3]=((((-1.0)*x429*x430))+(((-1.0)*x429*x431)));
9490 evalcond[4]=((((-1.0)*new_r01*x427*x429))+(((-1.0)*new_r11*x428*x429)));
9498 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9499 vinfos[0].jointtype = 17;
9500 vinfos[0].foffset = j0;
9501 vinfos[0].indices[0] = _ij0[0];
9502 vinfos[0].indices[1] = _ij0[1];
9503 vinfos[0].maxsolutions = _nj0;
9504 vinfos[1].jointtype = 1;
9505 vinfos[1].foffset = j1;
9506 vinfos[1].indices[0] = _ij1[0];
9507 vinfos[1].indices[1] = _ij1[1];
9508 vinfos[1].maxsolutions = _nj1;
9509 vinfos[2].jointtype = 1;
9510 vinfos[2].foffset = j2;
9511 vinfos[2].indices[0] = _ij2[0];
9512 vinfos[2].indices[1] = _ij2[1];
9513 vinfos[2].maxsolutions = _nj2;
9514 vinfos[3].jointtype = 1;
9515 vinfos[3].foffset = j3;
9516 vinfos[3].indices[0] = _ij3[0];
9517 vinfos[3].indices[1] = _ij3[1];
9518 vinfos[3].maxsolutions = _nj3;
9519 vinfos[4].jointtype = 1;
9520 vinfos[4].foffset = j4;
9521 vinfos[4].indices[0] = _ij4[0];
9522 vinfos[4].indices[1] = _ij4[1];
9523 vinfos[4].maxsolutions = _nj4;
9524 vinfos[5].jointtype = 1;
9525 vinfos[5].foffset = j5;
9526 vinfos[5].indices[0] = _ij5[0];
9527 vinfos[5].indices[1] = _ij5[1];
9528 vinfos[5].maxsolutions = _nj5;
9529 vinfos[6].jointtype = 1;
9530 vinfos[6].foffset = j6;
9531 vinfos[6].indices[0] = _ij6[0];
9532 vinfos[6].indices[1] = _ij6[1];
9533 vinfos[6].maxsolutions = _nj6;
9534 std::vector<int> vfree(0);
9546 if( bgotonextstatement )
9548 bool bgotonextstatement =
true;
9553 bgotonextstatement=
false;
9558 if( bgotonextstatement )
9571 IkReal j4array[1], cj4array[1], sj4array[1];
9572 bool j4valid[1]={
false};
9578 IkReal x433=x434.
value;
9585 j4array[0]=
IKatan2((x433*(x435.
value)*(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*new_r02*sj6))))), (new_r02*x433));
9586 sj4array[0]=
IKsin(j4array[0]);
9587 cj4array[0]=
IKcos(j4array[0]);
9588 if( j4array[0] >
IKPI )
9592 else if( j4array[0] < -
IKPI )
9593 { j4array[0]+=
IK2PI;
9596 for(
int ij4 = 0; ij4 < 1; ++ij4)
9602 _ij4[0] = ij4; _ij4[1] = -1;
9603 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
9607 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9610 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9612 IkReal evalcond[18];
9613 IkReal x436=
IKcos(j4);
9614 IkReal x437=
IKsin(j4);
9615 IkReal x438=((1.0)*sj6);
9616 IkReal x439=(cj5*sj6);
9617 IkReal x440=((1.0)*cj5);
9618 IkReal x441=((1.0)*sj5);
9619 IkReal x442=((1.0)*x436);
9620 IkReal x443=(new_r11*x437);
9621 IkReal x444=(new_r12*x437);
9622 IkReal x445=(cj6*x437);
9623 IkReal x446=(new_r10*x437);
9624 IkReal x447=((1.0)*x437);
9625 IkReal x448=(new_r00*x442);
9626 evalcond[0]=((((-1.0)*x436*x441))+new_r02);
9627 evalcond[1]=((((-1.0)*x437*x441))+new_r12);
9628 evalcond[2]=(((new_r12*x436))+(((-1.0)*new_r02*x447)));
9629 evalcond[3]=(((x436*x439))+x445+new_r01);
9630 evalcond[4]=((((-1.0)*x441))+x444+((new_r02*x436)));
9631 evalcond[5]=(((new_r01*x436))+x439+x443);
9632 evalcond[6]=(((sj6*x437))+new_r00+(((-1.0)*cj6*x436*x440)));
9633 evalcond[7]=((((-1.0)*cj6*x442))+new_r11+((x437*x439)));
9634 evalcond[8]=((((-1.0)*x438))+(((-1.0)*new_r00*x447))+((new_r10*x436)));
9635 evalcond[9]=((((-1.0)*new_r01*x447))+((new_r11*x436))+(((-1.0)*cj6)));
9636 evalcond[10]=(((new_r00*x436))+(((-1.0)*cj6*x440))+x446);
9637 evalcond[11]=((((-1.0)*x436*x438))+new_r10+(((-1.0)*x440*x445)));
9638 evalcond[12]=(((new_r22*sj5))+(((-1.0)*new_r02*x436*x440))+(((-1.0)*x440*x444)));
9639 evalcond[13]=((((-1.0)*new_r00*x436*x441))+(((-1.0)*new_r20*x440))+(((-1.0)*x441*x446)));
9640 evalcond[14]=((((-1.0)*new_r01*x436*x441))+(((-1.0)*new_r21*x440))+(((-1.0)*x441*x443)));
9641 evalcond[15]=(((new_r20*sj5))+cj6+(((-1.0)*new_r00*x436*x440))+(((-1.0)*x440*x446)));
9642 evalcond[16]=((1.0)+(((-1.0)*new_r02*x436*x441))+(((-1.0)*x441*x444))+(((-1.0)*new_r22*x440)));
9643 evalcond[17]=((((-1.0)*x438))+(((-1.0)*new_r01*x436*x440))+((new_r21*sj5))+(((-1.0)*x440*x443)));
9651 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9652 vinfos[0].jointtype = 17;
9653 vinfos[0].foffset = j0;
9654 vinfos[0].indices[0] = _ij0[0];
9655 vinfos[0].indices[1] = _ij0[1];
9656 vinfos[0].maxsolutions = _nj0;
9657 vinfos[1].jointtype = 1;
9658 vinfos[1].foffset = j1;
9659 vinfos[1].indices[0] = _ij1[0];
9660 vinfos[1].indices[1] = _ij1[1];
9661 vinfos[1].maxsolutions = _nj1;
9662 vinfos[2].jointtype = 1;
9663 vinfos[2].foffset = j2;
9664 vinfos[2].indices[0] = _ij2[0];
9665 vinfos[2].indices[1] = _ij2[1];
9666 vinfos[2].maxsolutions = _nj2;
9667 vinfos[3].jointtype = 1;
9668 vinfos[3].foffset = j3;
9669 vinfos[3].indices[0] = _ij3[0];
9670 vinfos[3].indices[1] = _ij3[1];
9671 vinfos[3].maxsolutions = _nj3;
9672 vinfos[4].jointtype = 1;
9673 vinfos[4].foffset = j4;
9674 vinfos[4].indices[0] = _ij4[0];
9675 vinfos[4].indices[1] = _ij4[1];
9676 vinfos[4].maxsolutions = _nj4;
9677 vinfos[5].jointtype = 1;
9678 vinfos[5].foffset = j5;
9679 vinfos[5].indices[0] = _ij5[0];
9680 vinfos[5].indices[1] = _ij5[1];
9681 vinfos[5].maxsolutions = _nj5;
9682 vinfos[6].jointtype = 1;
9683 vinfos[6].foffset = j6;
9684 vinfos[6].indices[0] = _ij6[0];
9685 vinfos[6].indices[1] = _ij6[1];
9686 vinfos[6].maxsolutions = _nj6;
9687 std::vector<int> vfree(0);
9700 IkReal j4array[1], cj4array[1], sj4array[1];
9701 bool j4valid[1]={
false};
9711 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x449.
value)))+(x450.
value));
9712 sj4array[0]=
IKsin(j4array[0]);
9713 cj4array[0]=
IKcos(j4array[0]);
9714 if( j4array[0] >
IKPI )
9718 else if( j4array[0] < -
IKPI )
9719 { j4array[0]+=
IK2PI;
9722 for(
int ij4 = 0; ij4 < 1; ++ij4)
9728 _ij4[0] = ij4; _ij4[1] = -1;
9729 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
9733 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9736 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9738 IkReal evalcond[18];
9739 IkReal x451=
IKcos(j4);
9740 IkReal x452=
IKsin(j4);
9741 IkReal x453=((1.0)*sj6);
9742 IkReal x454=(cj5*sj6);
9743 IkReal x455=((1.0)*cj5);
9744 IkReal x456=((1.0)*sj5);
9745 IkReal x457=((1.0)*x451);
9746 IkReal x458=(new_r11*x452);
9747 IkReal x459=(new_r12*x452);
9748 IkReal x460=(cj6*x452);
9749 IkReal x461=(new_r10*x452);
9750 IkReal x462=((1.0)*x452);
9751 IkReal x463=(new_r00*x457);
9752 evalcond[0]=((((-1.0)*x451*x456))+new_r02);
9753 evalcond[1]=((((-1.0)*x452*x456))+new_r12);
9754 evalcond[2]=(((new_r12*x451))+(((-1.0)*new_r02*x462)));
9755 evalcond[3]=(x460+new_r01+((x451*x454)));
9756 evalcond[4]=((((-1.0)*x456))+x459+((new_r02*x451)));
9757 evalcond[5]=(((new_r01*x451))+x458+x454);
9758 evalcond[6]=(((sj6*x452))+(((-1.0)*cj6*x451*x455))+new_r00);
9759 evalcond[7]=(((x452*x454))+(((-1.0)*cj6*x457))+new_r11);
9760 evalcond[8]=(((new_r10*x451))+(((-1.0)*x453))+(((-1.0)*new_r00*x462)));
9761 evalcond[9]=(((new_r11*x451))+(((-1.0)*new_r01*x462))+(((-1.0)*cj6)));
9762 evalcond[10]=(((new_r00*x451))+x461+(((-1.0)*cj6*x455)));
9763 evalcond[11]=((((-1.0)*x455*x460))+(((-1.0)*x451*x453))+new_r10);
9764 evalcond[12]=(((new_r22*sj5))+(((-1.0)*x455*x459))+(((-1.0)*new_r02*x451*x455)));
9765 evalcond[13]=((((-1.0)*new_r20*x455))+(((-1.0)*x456*x461))+(((-1.0)*new_r00*x451*x456)));
9766 evalcond[14]=((((-1.0)*new_r21*x455))+(((-1.0)*x456*x458))+(((-1.0)*new_r01*x451*x456)));
9767 evalcond[15]=(((new_r20*sj5))+cj6+(((-1.0)*new_r00*x451*x455))+(((-1.0)*x455*x461)));
9768 evalcond[16]=((1.0)+(((-1.0)*x456*x459))+(((-1.0)*new_r22*x455))+(((-1.0)*new_r02*x451*x456)));
9769 evalcond[17]=((((-1.0)*new_r01*x451*x455))+(((-1.0)*x453))+(((-1.0)*x455*x458))+((new_r21*sj5)));
9777 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9778 vinfos[0].jointtype = 17;
9779 vinfos[0].foffset = j0;
9780 vinfos[0].indices[0] = _ij0[0];
9781 vinfos[0].indices[1] = _ij0[1];
9782 vinfos[0].maxsolutions = _nj0;
9783 vinfos[1].jointtype = 1;
9784 vinfos[1].foffset = j1;
9785 vinfos[1].indices[0] = _ij1[0];
9786 vinfos[1].indices[1] = _ij1[1];
9787 vinfos[1].maxsolutions = _nj1;
9788 vinfos[2].jointtype = 1;
9789 vinfos[2].foffset = j2;
9790 vinfos[2].indices[0] = _ij2[0];
9791 vinfos[2].indices[1] = _ij2[1];
9792 vinfos[2].maxsolutions = _nj2;
9793 vinfos[3].jointtype = 1;
9794 vinfos[3].foffset = j3;
9795 vinfos[3].indices[0] = _ij3[0];
9796 vinfos[3].indices[1] = _ij3[1];
9797 vinfos[3].maxsolutions = _nj3;
9798 vinfos[4].jointtype = 1;
9799 vinfos[4].foffset = j4;
9800 vinfos[4].indices[0] = _ij4[0];
9801 vinfos[4].indices[1] = _ij4[1];
9802 vinfos[4].maxsolutions = _nj4;
9803 vinfos[5].jointtype = 1;
9804 vinfos[5].foffset = j5;
9805 vinfos[5].indices[0] = _ij5[0];
9806 vinfos[5].indices[1] = _ij5[1];
9807 vinfos[5].maxsolutions = _nj5;
9808 vinfos[6].jointtype = 1;
9809 vinfos[6].foffset = j6;
9810 vinfos[6].indices[0] = _ij6[0];
9811 vinfos[6].indices[1] = _ij6[1];
9812 vinfos[6].maxsolutions = _nj6;
9813 std::vector<int> vfree(0);
9832 IkReal j4array[1], cj4array[1], sj4array[1];
9833 bool j4valid[1]={
false};
9843 j4array[0]=((-1.5707963267949)+(((1.5707963267949)*(x464.
value)))+(x465.
value));
9844 sj4array[0]=
IKsin(j4array[0]);
9845 cj4array[0]=
IKcos(j4array[0]);
9846 if( j4array[0] >
IKPI )
9850 else if( j4array[0] < -
IKPI )
9851 { j4array[0]+=
IK2PI;
9854 for(
int ij4 = 0; ij4 < 1; ++ij4)
9860 _ij4[0] = ij4; _ij4[1] = -1;
9861 for(
int iij4 = ij4+1; iij4 < 1; ++iij4)
9865 j4valid[iij4]=
false; _ij4[1] = iij4;
break;
9868 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
9871 IkReal x466=
IKcos(j4);
9872 IkReal x467=
IKsin(j4);
9873 IkReal x468=((1.0)*sj5);
9874 IkReal x469=((1.0)*cj5);
9875 IkReal x470=(new_r12*x467);
9876 IkReal x471=(new_r02*x466);
9877 evalcond[0]=((((-1.0)*x466*x468))+new_r02);
9878 evalcond[1]=(new_r12+(((-1.0)*x467*x468)));
9879 evalcond[2]=(((new_r12*x466))+(((-1.0)*new_r02*x467)));
9880 evalcond[3]=((((-1.0)*x468))+x470+x471);
9881 evalcond[4]=((((-1.0)*x469*x470))+(((-1.0)*x469*x471))+((new_r22*sj5)));
9882 evalcond[5]=((((-1.0)*new_r10*x467*x468))+(((-1.0)*new_r20*x469))+(((-1.0)*new_r00*x466*x468)));
9883 evalcond[6]=((((-1.0)*new_r21*x469))+(((-1.0)*new_r01*x466*x468))+(((-1.0)*new_r11*x467*x468)));
9884 evalcond[7]=((1.0)+(((-1.0)*x468*x471))+(((-1.0)*x468*x470))+(((-1.0)*new_r22*x469)));
9894 j6eval[1]=((
IKabs(new_r20))+(
IKabs(new_r21)));
9896 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
9902 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
9909 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
9913 bool bgotonextstatement =
true;
9916 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j4)))), 6.28318530717959)));
9917 evalcond[1]=new_r02;
9918 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
9920 bgotonextstatement=
false;
9927 j6eval[1]=
IKsign(new_r12);
9928 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
9929 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
9938 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
9939 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
9947 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
9951 bool bgotonextstatement =
true;
9954 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
9955 evalcond[1]=new_r22;
9956 evalcond[2]=new_r11;
9957 evalcond[3]=new_r10;
9958 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
9960 bgotonextstatement=
false;
9962 IkReal j6array[1], cj6array[1], sj6array[1];
9963 bool j6valid[1]={
false};
9967 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
9968 sj6array[0]=
IKsin(j6array[0]);
9969 cj6array[0]=
IKcos(j6array[0]);
9970 if( j6array[0] >
IKPI )
9974 else if( j6array[0] < -
IKPI )
9975 { j6array[0]+=
IK2PI;
9978 for(
int ij6 = 0; ij6 < 1; ++ij6)
9984 _ij6[0] = ij6; _ij6[1] = -1;
9985 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
9989 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
9992 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
9995 IkReal x472=
IKcos(j6);
9996 IkReal x473=((1.0)*(
IKsin(j6)));
9997 evalcond[0]=(x472+new_r20);
9998 evalcond[1]=((((-1.0)*x473))+new_r21);
9999 evalcond[2]=((((-1.0)*x473))+(((-1.0)*new_r00)));
10000 evalcond[3]=((((-1.0)*x472))+(((-1.0)*new_r01)));
10008 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10009 vinfos[0].jointtype = 17;
10010 vinfos[0].foffset = j0;
10011 vinfos[0].indices[0] = _ij0[0];
10012 vinfos[0].indices[1] = _ij0[1];
10013 vinfos[0].maxsolutions = _nj0;
10014 vinfos[1].jointtype = 1;
10015 vinfos[1].foffset = j1;
10016 vinfos[1].indices[0] = _ij1[0];
10017 vinfos[1].indices[1] = _ij1[1];
10018 vinfos[1].maxsolutions = _nj1;
10019 vinfos[2].jointtype = 1;
10020 vinfos[2].foffset = j2;
10021 vinfos[2].indices[0] = _ij2[0];
10022 vinfos[2].indices[1] = _ij2[1];
10023 vinfos[2].maxsolutions = _nj2;
10024 vinfos[3].jointtype = 1;
10025 vinfos[3].foffset = j3;
10026 vinfos[3].indices[0] = _ij3[0];
10027 vinfos[3].indices[1] = _ij3[1];
10028 vinfos[3].maxsolutions = _nj3;
10029 vinfos[4].jointtype = 1;
10030 vinfos[4].foffset = j4;
10031 vinfos[4].indices[0] = _ij4[0];
10032 vinfos[4].indices[1] = _ij4[1];
10033 vinfos[4].maxsolutions = _nj4;
10034 vinfos[5].jointtype = 1;
10035 vinfos[5].foffset = j5;
10036 vinfos[5].indices[0] = _ij5[0];
10037 vinfos[5].indices[1] = _ij5[1];
10038 vinfos[5].maxsolutions = _nj5;
10039 vinfos[6].jointtype = 1;
10040 vinfos[6].foffset = j6;
10041 vinfos[6].indices[0] = _ij6[0];
10042 vinfos[6].indices[1] = _ij6[1];
10043 vinfos[6].maxsolutions = _nj6;
10044 std::vector<int> vfree(0);
10052 if( bgotonextstatement )
10054 bool bgotonextstatement =
true;
10057 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
10058 evalcond[1]=new_r22;
10059 evalcond[2]=new_r11;
10060 evalcond[3]=new_r10;
10061 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
10063 bgotonextstatement=
false;
10065 IkReal j6array[1], cj6array[1], sj6array[1];
10066 bool j6valid[1]={
false};
10070 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
10071 sj6array[0]=
IKsin(j6array[0]);
10072 cj6array[0]=
IKcos(j6array[0]);
10073 if( j6array[0] >
IKPI )
10077 else if( j6array[0] < -
IKPI )
10078 { j6array[0]+=
IK2PI;
10081 for(
int ij6 = 0; ij6 < 1; ++ij6)
10083 if( !j6valid[ij6] )
10087 _ij6[0] = ij6; _ij6[1] = -1;
10088 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10092 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10095 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10097 IkReal evalcond[4];
10098 IkReal x474=
IKsin(j6);
10099 IkReal x475=((1.0)*(
IKcos(j6)));
10100 evalcond[0]=(x474+new_r21);
10101 evalcond[1]=((((-1.0)*x475))+new_r20);
10102 evalcond[2]=((((-1.0)*x474))+(((-1.0)*new_r00)));
10103 evalcond[3]=((((-1.0)*x475))+(((-1.0)*new_r01)));
10111 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10112 vinfos[0].jointtype = 17;
10113 vinfos[0].foffset = j0;
10114 vinfos[0].indices[0] = _ij0[0];
10115 vinfos[0].indices[1] = _ij0[1];
10116 vinfos[0].maxsolutions = _nj0;
10117 vinfos[1].jointtype = 1;
10118 vinfos[1].foffset = j1;
10119 vinfos[1].indices[0] = _ij1[0];
10120 vinfos[1].indices[1] = _ij1[1];
10121 vinfos[1].maxsolutions = _nj1;
10122 vinfos[2].jointtype = 1;
10123 vinfos[2].foffset = j2;
10124 vinfos[2].indices[0] = _ij2[0];
10125 vinfos[2].indices[1] = _ij2[1];
10126 vinfos[2].maxsolutions = _nj2;
10127 vinfos[3].jointtype = 1;
10128 vinfos[3].foffset = j3;
10129 vinfos[3].indices[0] = _ij3[0];
10130 vinfos[3].indices[1] = _ij3[1];
10131 vinfos[3].maxsolutions = _nj3;
10132 vinfos[4].jointtype = 1;
10133 vinfos[4].foffset = j4;
10134 vinfos[4].indices[0] = _ij4[0];
10135 vinfos[4].indices[1] = _ij4[1];
10136 vinfos[4].maxsolutions = _nj4;
10137 vinfos[5].jointtype = 1;
10138 vinfos[5].foffset = j5;
10139 vinfos[5].indices[0] = _ij5[0];
10140 vinfos[5].indices[1] = _ij5[1];
10141 vinfos[5].maxsolutions = _nj5;
10142 vinfos[6].jointtype = 1;
10143 vinfos[6].foffset = j6;
10144 vinfos[6].indices[0] = _ij6[0];
10145 vinfos[6].indices[1] = _ij6[1];
10146 vinfos[6].maxsolutions = _nj6;
10147 std::vector<int> vfree(0);
10155 if( bgotonextstatement )
10157 bool bgotonextstatement =
true;
10160 evalcond[0]=
IKabs(new_r12);
10161 evalcond[1]=new_r20;
10162 evalcond[2]=new_r21;
10163 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
10165 bgotonextstatement=
false;
10170 j4=1.5707963267949;
10174 j6eval[2]=((
IKabs(new_r11))+(
IKabs(new_r10)));
10175 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
10181 j4=1.5707963267949;
10184 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
10187 IkReal evalcond[3];
10188 bool bgotonextstatement =
true;
10191 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
10192 evalcond[1]=new_r11;
10193 evalcond[2]=new_r10;
10194 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
10196 bgotonextstatement=
false;
10198 IkReal j6array[1], cj6array[1], sj6array[1];
10199 bool j6valid[1]={
false};
10203 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
10204 sj6array[0]=
IKsin(j6array[0]);
10205 cj6array[0]=
IKcos(j6array[0]);
10206 if( j6array[0] >
IKPI )
10210 else if( j6array[0] < -
IKPI )
10211 { j6array[0]+=
IK2PI;
10214 for(
int ij6 = 0; ij6 < 1; ++ij6)
10216 if( !j6valid[ij6] )
10220 _ij6[0] = ij6; _ij6[1] = -1;
10221 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10225 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10228 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10230 IkReal evalcond[4];
10231 IkReal x476=
IKsin(j6);
10232 IkReal x477=
IKcos(j6);
10234 evalcond[1]=((-1.0)*x476);
10235 evalcond[2]=((((-1.0)*x476))+(((-1.0)*new_r00)));
10236 evalcond[3]=((((-1.0)*x477))+(((-1.0)*new_r01)));
10244 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10245 vinfos[0].jointtype = 17;
10246 vinfos[0].foffset = j0;
10247 vinfos[0].indices[0] = _ij0[0];
10248 vinfos[0].indices[1] = _ij0[1];
10249 vinfos[0].maxsolutions = _nj0;
10250 vinfos[1].jointtype = 1;
10251 vinfos[1].foffset = j1;
10252 vinfos[1].indices[0] = _ij1[0];
10253 vinfos[1].indices[1] = _ij1[1];
10254 vinfos[1].maxsolutions = _nj1;
10255 vinfos[2].jointtype = 1;
10256 vinfos[2].foffset = j2;
10257 vinfos[2].indices[0] = _ij2[0];
10258 vinfos[2].indices[1] = _ij2[1];
10259 vinfos[2].maxsolutions = _nj2;
10260 vinfos[3].jointtype = 1;
10261 vinfos[3].foffset = j3;
10262 vinfos[3].indices[0] = _ij3[0];
10263 vinfos[3].indices[1] = _ij3[1];
10264 vinfos[3].maxsolutions = _nj3;
10265 vinfos[4].jointtype = 1;
10266 vinfos[4].foffset = j4;
10267 vinfos[4].indices[0] = _ij4[0];
10268 vinfos[4].indices[1] = _ij4[1];
10269 vinfos[4].maxsolutions = _nj4;
10270 vinfos[5].jointtype = 1;
10271 vinfos[5].foffset = j5;
10272 vinfos[5].indices[0] = _ij5[0];
10273 vinfos[5].indices[1] = _ij5[1];
10274 vinfos[5].maxsolutions = _nj5;
10275 vinfos[6].jointtype = 1;
10276 vinfos[6].foffset = j6;
10277 vinfos[6].indices[0] = _ij6[0];
10278 vinfos[6].indices[1] = _ij6[1];
10279 vinfos[6].maxsolutions = _nj6;
10280 std::vector<int> vfree(0);
10288 if( bgotonextstatement )
10290 bool bgotonextstatement =
true;
10293 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
10294 evalcond[1]=new_r11;
10295 evalcond[2]=new_r10;
10296 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
10298 bgotonextstatement=
false;
10300 IkReal j6array[1], cj6array[1], sj6array[1];
10301 bool j6valid[1]={
false};
10305 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
10306 sj6array[0]=
IKsin(j6array[0]);
10307 cj6array[0]=
IKcos(j6array[0]);
10308 if( j6array[0] >
IKPI )
10312 else if( j6array[0] < -
IKPI )
10313 { j6array[0]+=
IK2PI;
10316 for(
int ij6 = 0; ij6 < 1; ++ij6)
10318 if( !j6valid[ij6] )
10322 _ij6[0] = ij6; _ij6[1] = -1;
10323 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10327 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10330 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10332 IkReal evalcond[4];
10333 IkReal x478=
IKsin(j6);
10334 IkReal x479=
IKcos(j6);
10336 evalcond[1]=((-1.0)*x478);
10337 evalcond[2]=((((-1.0)*x478))+(((-1.0)*new_r00)));
10338 evalcond[3]=((((-1.0)*x479))+(((-1.0)*new_r01)));
10346 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10347 vinfos[0].jointtype = 17;
10348 vinfos[0].foffset = j0;
10349 vinfos[0].indices[0] = _ij0[0];
10350 vinfos[0].indices[1] = _ij0[1];
10351 vinfos[0].maxsolutions = _nj0;
10352 vinfos[1].jointtype = 1;
10353 vinfos[1].foffset = j1;
10354 vinfos[1].indices[0] = _ij1[0];
10355 vinfos[1].indices[1] = _ij1[1];
10356 vinfos[1].maxsolutions = _nj1;
10357 vinfos[2].jointtype = 1;
10358 vinfos[2].foffset = j2;
10359 vinfos[2].indices[0] = _ij2[0];
10360 vinfos[2].indices[1] = _ij2[1];
10361 vinfos[2].maxsolutions = _nj2;
10362 vinfos[3].jointtype = 1;
10363 vinfos[3].foffset = j3;
10364 vinfos[3].indices[0] = _ij3[0];
10365 vinfos[3].indices[1] = _ij3[1];
10366 vinfos[3].maxsolutions = _nj3;
10367 vinfos[4].jointtype = 1;
10368 vinfos[4].foffset = j4;
10369 vinfos[4].indices[0] = _ij4[0];
10370 vinfos[4].indices[1] = _ij4[1];
10371 vinfos[4].maxsolutions = _nj4;
10372 vinfos[5].jointtype = 1;
10373 vinfos[5].foffset = j5;
10374 vinfos[5].indices[0] = _ij5[0];
10375 vinfos[5].indices[1] = _ij5[1];
10376 vinfos[5].maxsolutions = _nj5;
10377 vinfos[6].jointtype = 1;
10378 vinfos[6].foffset = j6;
10379 vinfos[6].indices[0] = _ij6[0];
10380 vinfos[6].indices[1] = _ij6[1];
10381 vinfos[6].maxsolutions = _nj6;
10382 std::vector<int> vfree(0);
10390 if( bgotonextstatement )
10392 bool bgotonextstatement =
true;
10395 evalcond[0]=((
IKabs(new_r11))+(
IKabs(new_r10)));
10396 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
10398 bgotonextstatement=
false;
10400 IkReal j6array[1], cj6array[1], sj6array[1];
10401 bool j6valid[1]={
false};
10405 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
10406 sj6array[0]=
IKsin(j6array[0]);
10407 cj6array[0]=
IKcos(j6array[0]);
10408 if( j6array[0] >
IKPI )
10412 else if( j6array[0] < -
IKPI )
10413 { j6array[0]+=
IK2PI;
10416 for(
int ij6 = 0; ij6 < 1; ++ij6)
10418 if( !j6valid[ij6] )
10422 _ij6[0] = ij6; _ij6[1] = -1;
10423 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10427 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10430 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10432 IkReal evalcond[6];
10433 IkReal x480=
IKsin(j6);
10434 IkReal x481=
IKcos(j6);
10436 evalcond[1]=(cj5*x480);
10437 evalcond[2]=((-1.0)*x480);
10438 evalcond[3]=((-1.0)*cj5*x481);
10439 evalcond[4]=((((-1.0)*x480))+(((-1.0)*new_r00)));
10440 evalcond[5]=((((-1.0)*x481))+(((-1.0)*new_r01)));
10448 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10449 vinfos[0].jointtype = 17;
10450 vinfos[0].foffset = j0;
10451 vinfos[0].indices[0] = _ij0[0];
10452 vinfos[0].indices[1] = _ij0[1];
10453 vinfos[0].maxsolutions = _nj0;
10454 vinfos[1].jointtype = 1;
10455 vinfos[1].foffset = j1;
10456 vinfos[1].indices[0] = _ij1[0];
10457 vinfos[1].indices[1] = _ij1[1];
10458 vinfos[1].maxsolutions = _nj1;
10459 vinfos[2].jointtype = 1;
10460 vinfos[2].foffset = j2;
10461 vinfos[2].indices[0] = _ij2[0];
10462 vinfos[2].indices[1] = _ij2[1];
10463 vinfos[2].maxsolutions = _nj2;
10464 vinfos[3].jointtype = 1;
10465 vinfos[3].foffset = j3;
10466 vinfos[3].indices[0] = _ij3[0];
10467 vinfos[3].indices[1] = _ij3[1];
10468 vinfos[3].maxsolutions = _nj3;
10469 vinfos[4].jointtype = 1;
10470 vinfos[4].foffset = j4;
10471 vinfos[4].indices[0] = _ij4[0];
10472 vinfos[4].indices[1] = _ij4[1];
10473 vinfos[4].maxsolutions = _nj4;
10474 vinfos[5].jointtype = 1;
10475 vinfos[5].foffset = j5;
10476 vinfos[5].indices[0] = _ij5[0];
10477 vinfos[5].indices[1] = _ij5[1];
10478 vinfos[5].maxsolutions = _nj5;
10479 vinfos[6].jointtype = 1;
10480 vinfos[6].foffset = j6;
10481 vinfos[6].indices[0] = _ij6[0];
10482 vinfos[6].indices[1] = _ij6[1];
10483 vinfos[6].maxsolutions = _nj6;
10484 std::vector<int> vfree(0);
10492 if( bgotonextstatement )
10494 bool bgotonextstatement =
true;
10499 bgotonextstatement=
false;
10504 if( bgotonextstatement )
10515 IkReal j6array[1], cj6array[1], sj6array[1];
10516 bool j6valid[1]={
false};
10524 j6array[0]=
IKatan2(((-1.0)*new_r00), (new_r10*(x482.
value)));
10525 sj6array[0]=
IKsin(j6array[0]);
10526 cj6array[0]=
IKcos(j6array[0]);
10527 if( j6array[0] >
IKPI )
10531 else if( j6array[0] < -
IKPI )
10532 { j6array[0]+=
IK2PI;
10535 for(
int ij6 = 0; ij6 < 1; ++ij6)
10537 if( !j6valid[ij6] )
10541 _ij6[0] = ij6; _ij6[1] = -1;
10542 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10546 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10549 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10551 IkReal evalcond[6];
10552 IkReal x483=
IKcos(j6);
10553 IkReal x484=
IKsin(j6);
10554 IkReal x485=((1.0)*cj5);
10555 IkReal x486=((1.0)*x484);
10556 evalcond[0]=(((cj5*x484))+new_r11);
10557 evalcond[1]=((((-1.0)*x483*x485))+new_r10);
10558 evalcond[2]=((((-1.0)*new_r10*x485))+x483);
10559 evalcond[3]=((((-1.0)*x486))+(((-1.0)*new_r00)));
10560 evalcond[4]=((((-1.0)*x483))+(((-1.0)*new_r01)));
10561 evalcond[5]=((((-1.0)*new_r11*x485))+(((-1.0)*x486)));
10569 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10570 vinfos[0].jointtype = 17;
10571 vinfos[0].foffset = j0;
10572 vinfos[0].indices[0] = _ij0[0];
10573 vinfos[0].indices[1] = _ij0[1];
10574 vinfos[0].maxsolutions = _nj0;
10575 vinfos[1].jointtype = 1;
10576 vinfos[1].foffset = j1;
10577 vinfos[1].indices[0] = _ij1[0];
10578 vinfos[1].indices[1] = _ij1[1];
10579 vinfos[1].maxsolutions = _nj1;
10580 vinfos[2].jointtype = 1;
10581 vinfos[2].foffset = j2;
10582 vinfos[2].indices[0] = _ij2[0];
10583 vinfos[2].indices[1] = _ij2[1];
10584 vinfos[2].maxsolutions = _nj2;
10585 vinfos[3].jointtype = 1;
10586 vinfos[3].foffset = j3;
10587 vinfos[3].indices[0] = _ij3[0];
10588 vinfos[3].indices[1] = _ij3[1];
10589 vinfos[3].maxsolutions = _nj3;
10590 vinfos[4].jointtype = 1;
10591 vinfos[4].foffset = j4;
10592 vinfos[4].indices[0] = _ij4[0];
10593 vinfos[4].indices[1] = _ij4[1];
10594 vinfos[4].maxsolutions = _nj4;
10595 vinfos[5].jointtype = 1;
10596 vinfos[5].foffset = j5;
10597 vinfos[5].indices[0] = _ij5[0];
10598 vinfos[5].indices[1] = _ij5[1];
10599 vinfos[5].maxsolutions = _nj5;
10600 vinfos[6].jointtype = 1;
10601 vinfos[6].foffset = j6;
10602 vinfos[6].indices[0] = _ij6[0];
10603 vinfos[6].indices[1] = _ij6[1];
10604 vinfos[6].maxsolutions = _nj6;
10605 std::vector<int> vfree(0);
10618 IkReal j6array[1], cj6array[1], sj6array[1];
10619 bool j6valid[1]={
false};
10629 j6array[0]=((-1.5707963267949)+(x487.
value)+(((1.5707963267949)*(x488.
value))));
10630 sj6array[0]=
IKsin(j6array[0]);
10631 cj6array[0]=
IKcos(j6array[0]);
10632 if( j6array[0] >
IKPI )
10636 else if( j6array[0] < -
IKPI )
10637 { j6array[0]+=
IK2PI;
10640 for(
int ij6 = 0; ij6 < 1; ++ij6)
10642 if( !j6valid[ij6] )
10646 _ij6[0] = ij6; _ij6[1] = -1;
10647 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10651 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10654 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10656 IkReal evalcond[6];
10657 IkReal x489=
IKcos(j6);
10658 IkReal x490=
IKsin(j6);
10659 IkReal x491=((1.0)*cj5);
10660 IkReal x492=((1.0)*x490);
10661 evalcond[0]=(((cj5*x490))+new_r11);
10662 evalcond[1]=((((-1.0)*x489*x491))+new_r10);
10663 evalcond[2]=((((-1.0)*new_r10*x491))+x489);
10664 evalcond[3]=((((-1.0)*x492))+(((-1.0)*new_r00)));
10665 evalcond[4]=((((-1.0)*x489))+(((-1.0)*new_r01)));
10666 evalcond[5]=((((-1.0)*x492))+(((-1.0)*new_r11*x491)));
10674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10675 vinfos[0].jointtype = 17;
10676 vinfos[0].foffset = j0;
10677 vinfos[0].indices[0] = _ij0[0];
10678 vinfos[0].indices[1] = _ij0[1];
10679 vinfos[0].maxsolutions = _nj0;
10680 vinfos[1].jointtype = 1;
10681 vinfos[1].foffset = j1;
10682 vinfos[1].indices[0] = _ij1[0];
10683 vinfos[1].indices[1] = _ij1[1];
10684 vinfos[1].maxsolutions = _nj1;
10685 vinfos[2].jointtype = 1;
10686 vinfos[2].foffset = j2;
10687 vinfos[2].indices[0] = _ij2[0];
10688 vinfos[2].indices[1] = _ij2[1];
10689 vinfos[2].maxsolutions = _nj2;
10690 vinfos[3].jointtype = 1;
10691 vinfos[3].foffset = j3;
10692 vinfos[3].indices[0] = _ij3[0];
10693 vinfos[3].indices[1] = _ij3[1];
10694 vinfos[3].maxsolutions = _nj3;
10695 vinfos[4].jointtype = 1;
10696 vinfos[4].foffset = j4;
10697 vinfos[4].indices[0] = _ij4[0];
10698 vinfos[4].indices[1] = _ij4[1];
10699 vinfos[4].maxsolutions = _nj4;
10700 vinfos[5].jointtype = 1;
10701 vinfos[5].foffset = j5;
10702 vinfos[5].indices[0] = _ij5[0];
10703 vinfos[5].indices[1] = _ij5[1];
10704 vinfos[5].maxsolutions = _nj5;
10705 vinfos[6].jointtype = 1;
10706 vinfos[6].foffset = j6;
10707 vinfos[6].indices[0] = _ij6[0];
10708 vinfos[6].indices[1] = _ij6[1];
10709 vinfos[6].maxsolutions = _nj6;
10710 std::vector<int> vfree(0);
10722 if( bgotonextstatement )
10724 bool bgotonextstatement =
true;
10727 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
10728 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
10730 bgotonextstatement=
false;
10732 IkReal j6array[1], cj6array[1], sj6array[1];
10733 bool j6valid[1]={
false};
10737 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
10738 sj6array[0]=
IKsin(j6array[0]);
10739 cj6array[0]=
IKcos(j6array[0]);
10740 if( j6array[0] >
IKPI )
10744 else if( j6array[0] < -
IKPI )
10745 { j6array[0]+=
IK2PI;
10748 for(
int ij6 = 0; ij6 < 1; ++ij6)
10750 if( !j6valid[ij6] )
10754 _ij6[0] = ij6; _ij6[1] = -1;
10755 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10759 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10762 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10764 IkReal evalcond[6];
10765 IkReal x493=
IKsin(j6);
10766 IkReal x494=
IKcos(j6);
10768 evalcond[1]=(new_r22*x493);
10769 evalcond[2]=((-1.0)*x493);
10770 evalcond[3]=((-1.0)*new_r22*x494);
10771 evalcond[4]=((((-1.0)*x493))+(((-1.0)*new_r00)));
10772 evalcond[5]=((((-1.0)*x494))+(((-1.0)*new_r01)));
10780 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10781 vinfos[0].jointtype = 17;
10782 vinfos[0].foffset = j0;
10783 vinfos[0].indices[0] = _ij0[0];
10784 vinfos[0].indices[1] = _ij0[1];
10785 vinfos[0].maxsolutions = _nj0;
10786 vinfos[1].jointtype = 1;
10787 vinfos[1].foffset = j1;
10788 vinfos[1].indices[0] = _ij1[0];
10789 vinfos[1].indices[1] = _ij1[1];
10790 vinfos[1].maxsolutions = _nj1;
10791 vinfos[2].jointtype = 1;
10792 vinfos[2].foffset = j2;
10793 vinfos[2].indices[0] = _ij2[0];
10794 vinfos[2].indices[1] = _ij2[1];
10795 vinfos[2].maxsolutions = _nj2;
10796 vinfos[3].jointtype = 1;
10797 vinfos[3].foffset = j3;
10798 vinfos[3].indices[0] = _ij3[0];
10799 vinfos[3].indices[1] = _ij3[1];
10800 vinfos[3].maxsolutions = _nj3;
10801 vinfos[4].jointtype = 1;
10802 vinfos[4].foffset = j4;
10803 vinfos[4].indices[0] = _ij4[0];
10804 vinfos[4].indices[1] = _ij4[1];
10805 vinfos[4].maxsolutions = _nj4;
10806 vinfos[5].jointtype = 1;
10807 vinfos[5].foffset = j5;
10808 vinfos[5].indices[0] = _ij5[0];
10809 vinfos[5].indices[1] = _ij5[1];
10810 vinfos[5].maxsolutions = _nj5;
10811 vinfos[6].jointtype = 1;
10812 vinfos[6].foffset = j6;
10813 vinfos[6].indices[0] = _ij6[0];
10814 vinfos[6].indices[1] = _ij6[1];
10815 vinfos[6].maxsolutions = _nj6;
10816 std::vector<int> vfree(0);
10824 if( bgotonextstatement )
10826 bool bgotonextstatement =
true;
10831 bgotonextstatement=
false;
10836 if( bgotonextstatement )
10848 IkReal j6array[1], cj6array[1], sj6array[1];
10849 bool j6valid[1]={
false};
10857 j6array[0]=
IKatan2(((-1.0)*new_r00), ((-1.0)*new_r20*(x495.
value)));
10858 sj6array[0]=
IKsin(j6array[0]);
10859 cj6array[0]=
IKcos(j6array[0]);
10860 if( j6array[0] >
IKPI )
10864 else if( j6array[0] < -
IKPI )
10865 { j6array[0]+=
IK2PI;
10868 for(
int ij6 = 0; ij6 < 1; ++ij6)
10870 if( !j6valid[ij6] )
10874 _ij6[0] = ij6; _ij6[1] = -1;
10875 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10879 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10882 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10884 IkReal evalcond[8];
10885 IkReal x496=
IKsin(j6);
10886 IkReal x497=
IKcos(j6);
10887 IkReal x498=((1.0)*cj5);
10888 IkReal x499=((1.0)*x496);
10889 evalcond[0]=(((new_r12*x497))+new_r20);
10890 evalcond[1]=(((cj5*x496))+new_r11);
10891 evalcond[2]=(new_r21+(((-1.0)*new_r12*x499)));
10892 evalcond[3]=((((-1.0)*x497*x498))+new_r10);
10893 evalcond[4]=((((-1.0)*x499))+(((-1.0)*new_r00)));
10894 evalcond[5]=((((-1.0)*x497))+(((-1.0)*new_r01)));
10895 evalcond[6]=(((new_r20*sj5))+(((-1.0)*new_r10*x498))+x497);
10896 evalcond[7]=((((-1.0)*x499))+(((-1.0)*new_r11*x498))+((new_r21*sj5)));
10904 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
10905 vinfos[0].jointtype = 17;
10906 vinfos[0].foffset = j0;
10907 vinfos[0].indices[0] = _ij0[0];
10908 vinfos[0].indices[1] = _ij0[1];
10909 vinfos[0].maxsolutions = _nj0;
10910 vinfos[1].jointtype = 1;
10911 vinfos[1].foffset = j1;
10912 vinfos[1].indices[0] = _ij1[0];
10913 vinfos[1].indices[1] = _ij1[1];
10914 vinfos[1].maxsolutions = _nj1;
10915 vinfos[2].jointtype = 1;
10916 vinfos[2].foffset = j2;
10917 vinfos[2].indices[0] = _ij2[0];
10918 vinfos[2].indices[1] = _ij2[1];
10919 vinfos[2].maxsolutions = _nj2;
10920 vinfos[3].jointtype = 1;
10921 vinfos[3].foffset = j3;
10922 vinfos[3].indices[0] = _ij3[0];
10923 vinfos[3].indices[1] = _ij3[1];
10924 vinfos[3].maxsolutions = _nj3;
10925 vinfos[4].jointtype = 1;
10926 vinfos[4].foffset = j4;
10927 vinfos[4].indices[0] = _ij4[0];
10928 vinfos[4].indices[1] = _ij4[1];
10929 vinfos[4].maxsolutions = _nj4;
10930 vinfos[5].jointtype = 1;
10931 vinfos[5].foffset = j5;
10932 vinfos[5].indices[0] = _ij5[0];
10933 vinfos[5].indices[1] = _ij5[1];
10934 vinfos[5].maxsolutions = _nj5;
10935 vinfos[6].jointtype = 1;
10936 vinfos[6].foffset = j6;
10937 vinfos[6].indices[0] = _ij6[0];
10938 vinfos[6].indices[1] = _ij6[1];
10939 vinfos[6].maxsolutions = _nj6;
10940 std::vector<int> vfree(0);
10953 IkReal j6array[1], cj6array[1], sj6array[1];
10954 bool j6valid[1]={
false};
10964 j6array[0]=((-1.5707963267949)+(x500.
value)+(((1.5707963267949)*(x501.
value))));
10965 sj6array[0]=
IKsin(j6array[0]);
10966 cj6array[0]=
IKcos(j6array[0]);
10967 if( j6array[0] >
IKPI )
10971 else if( j6array[0] < -
IKPI )
10972 { j6array[0]+=
IK2PI;
10975 for(
int ij6 = 0; ij6 < 1; ++ij6)
10977 if( !j6valid[ij6] )
10981 _ij6[0] = ij6; _ij6[1] = -1;
10982 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
10986 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
10989 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
10991 IkReal evalcond[8];
10992 IkReal x502=
IKsin(j6);
10993 IkReal x503=
IKcos(j6);
10994 IkReal x504=((1.0)*cj5);
10995 IkReal x505=((1.0)*x502);
10996 evalcond[0]=(((new_r12*x503))+new_r20);
10997 evalcond[1]=(((cj5*x502))+new_r11);
10998 evalcond[2]=(new_r21+(((-1.0)*new_r12*x505)));
10999 evalcond[3]=((((-1.0)*x503*x504))+new_r10);
11000 evalcond[4]=((((-1.0)*x505))+(((-1.0)*new_r00)));
11001 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x503)));
11002 evalcond[6]=(((new_r20*sj5))+x503+(((-1.0)*new_r10*x504)));
11003 evalcond[7]=((((-1.0)*x505))+(((-1.0)*new_r11*x504))+((new_r21*sj5)));
11011 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11012 vinfos[0].jointtype = 17;
11013 vinfos[0].foffset = j0;
11014 vinfos[0].indices[0] = _ij0[0];
11015 vinfos[0].indices[1] = _ij0[1];
11016 vinfos[0].maxsolutions = _nj0;
11017 vinfos[1].jointtype = 1;
11018 vinfos[1].foffset = j1;
11019 vinfos[1].indices[0] = _ij1[0];
11020 vinfos[1].indices[1] = _ij1[1];
11021 vinfos[1].maxsolutions = _nj1;
11022 vinfos[2].jointtype = 1;
11023 vinfos[2].foffset = j2;
11024 vinfos[2].indices[0] = _ij2[0];
11025 vinfos[2].indices[1] = _ij2[1];
11026 vinfos[2].maxsolutions = _nj2;
11027 vinfos[3].jointtype = 1;
11028 vinfos[3].foffset = j3;
11029 vinfos[3].indices[0] = _ij3[0];
11030 vinfos[3].indices[1] = _ij3[1];
11031 vinfos[3].maxsolutions = _nj3;
11032 vinfos[4].jointtype = 1;
11033 vinfos[4].foffset = j4;
11034 vinfos[4].indices[0] = _ij4[0];
11035 vinfos[4].indices[1] = _ij4[1];
11036 vinfos[4].maxsolutions = _nj4;
11037 vinfos[5].jointtype = 1;
11038 vinfos[5].foffset = j5;
11039 vinfos[5].indices[0] = _ij5[0];
11040 vinfos[5].indices[1] = _ij5[1];
11041 vinfos[5].maxsolutions = _nj5;
11042 vinfos[6].jointtype = 1;
11043 vinfos[6].foffset = j6;
11044 vinfos[6].indices[0] = _ij6[0];
11045 vinfos[6].indices[1] = _ij6[1];
11046 vinfos[6].maxsolutions = _nj6;
11047 std::vector<int> vfree(0);
11060 IkReal j6array[1], cj6array[1], sj6array[1];
11061 bool j6valid[1]={
false};
11071 j6array[0]=((-1.5707963267949)+(x506.
value)+(((1.5707963267949)*(x507.
value))));
11072 sj6array[0]=
IKsin(j6array[0]);
11073 cj6array[0]=
IKcos(j6array[0]);
11074 if( j6array[0] >
IKPI )
11078 else if( j6array[0] < -
IKPI )
11079 { j6array[0]+=
IK2PI;
11082 for(
int ij6 = 0; ij6 < 1; ++ij6)
11084 if( !j6valid[ij6] )
11088 _ij6[0] = ij6; _ij6[1] = -1;
11089 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11093 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11096 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11098 IkReal evalcond[8];
11099 IkReal x508=
IKsin(j6);
11100 IkReal x509=
IKcos(j6);
11101 IkReal x510=((1.0)*cj5);
11102 IkReal x511=((1.0)*x508);
11103 evalcond[0]=(((new_r12*x509))+new_r20);
11104 evalcond[1]=(((cj5*x508))+new_r11);
11105 evalcond[2]=((((-1.0)*new_r12*x511))+new_r21);
11106 evalcond[3]=(new_r10+(((-1.0)*x509*x510)));
11107 evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x511)));
11108 evalcond[5]=((((-1.0)*new_r01))+(((-1.0)*x509)));
11109 evalcond[6]=(((new_r20*sj5))+(((-1.0)*new_r10*x510))+x509);
11110 evalcond[7]=((((-1.0)*new_r11*x510))+((new_r21*sj5))+(((-1.0)*x511)));
11118 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11119 vinfos[0].jointtype = 17;
11120 vinfos[0].foffset = j0;
11121 vinfos[0].indices[0] = _ij0[0];
11122 vinfos[0].indices[1] = _ij0[1];
11123 vinfos[0].maxsolutions = _nj0;
11124 vinfos[1].jointtype = 1;
11125 vinfos[1].foffset = j1;
11126 vinfos[1].indices[0] = _ij1[0];
11127 vinfos[1].indices[1] = _ij1[1];
11128 vinfos[1].maxsolutions = _nj1;
11129 vinfos[2].jointtype = 1;
11130 vinfos[2].foffset = j2;
11131 vinfos[2].indices[0] = _ij2[0];
11132 vinfos[2].indices[1] = _ij2[1];
11133 vinfos[2].maxsolutions = _nj2;
11134 vinfos[3].jointtype = 1;
11135 vinfos[3].foffset = j3;
11136 vinfos[3].indices[0] = _ij3[0];
11137 vinfos[3].indices[1] = _ij3[1];
11138 vinfos[3].maxsolutions = _nj3;
11139 vinfos[4].jointtype = 1;
11140 vinfos[4].foffset = j4;
11141 vinfos[4].indices[0] = _ij4[0];
11142 vinfos[4].indices[1] = _ij4[1];
11143 vinfos[4].maxsolutions = _nj4;
11144 vinfos[5].jointtype = 1;
11145 vinfos[5].foffset = j5;
11146 vinfos[5].indices[0] = _ij5[0];
11147 vinfos[5].indices[1] = _ij5[1];
11148 vinfos[5].maxsolutions = _nj5;
11149 vinfos[6].jointtype = 1;
11150 vinfos[6].foffset = j6;
11151 vinfos[6].indices[0] = _ij6[0];
11152 vinfos[6].indices[1] = _ij6[1];
11153 vinfos[6].maxsolutions = _nj6;
11154 std::vector<int> vfree(0);
11166 if( bgotonextstatement )
11168 bool bgotonextstatement =
true;
11171 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j4)))), 6.28318530717959)));
11172 evalcond[1]=new_r02;
11173 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
11175 bgotonextstatement=
false;
11177 IkReal j6array[1], cj6array[1], sj6array[1];
11178 bool j6valid[1]={
false};
11182 j6array[0]=
IKatan2(new_r00, new_r01);
11183 sj6array[0]=
IKsin(j6array[0]);
11184 cj6array[0]=
IKcos(j6array[0]);
11185 if( j6array[0] >
IKPI )
11189 else if( j6array[0] < -
IKPI )
11190 { j6array[0]+=
IK2PI;
11193 for(
int ij6 = 0; ij6 < 1; ++ij6)
11195 if( !j6valid[ij6] )
11199 _ij6[0] = ij6; _ij6[1] = -1;
11200 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11204 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11207 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11209 IkReal evalcond[8];
11210 IkReal x512=
IKsin(j6);
11211 IkReal x513=
IKcos(j6);
11212 IkReal x514=((1.0)*x512);
11213 IkReal x515=((1.0)*x513);
11214 evalcond[0]=(((new_r12*x512))+new_r21);
11215 evalcond[1]=(new_r00+(((-1.0)*x514)));
11216 evalcond[2]=(new_r01+(((-1.0)*x515)));
11217 evalcond[3]=((((-1.0)*new_r12*x515))+new_r20);
11218 evalcond[4]=(((cj5*x512))+(((-1.0)*new_r11)));
11219 evalcond[5]=((((-1.0)*cj5*x515))+(((-1.0)*new_r10)));
11220 evalcond[6]=(((new_r20*sj5))+((cj5*new_r10))+x513);
11221 evalcond[7]=(((cj5*new_r11))+((new_r21*sj5))+(((-1.0)*x514)));
11229 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11230 vinfos[0].jointtype = 17;
11231 vinfos[0].foffset = j0;
11232 vinfos[0].indices[0] = _ij0[0];
11233 vinfos[0].indices[1] = _ij0[1];
11234 vinfos[0].maxsolutions = _nj0;
11235 vinfos[1].jointtype = 1;
11236 vinfos[1].foffset = j1;
11237 vinfos[1].indices[0] = _ij1[0];
11238 vinfos[1].indices[1] = _ij1[1];
11239 vinfos[1].maxsolutions = _nj1;
11240 vinfos[2].jointtype = 1;
11241 vinfos[2].foffset = j2;
11242 vinfos[2].indices[0] = _ij2[0];
11243 vinfos[2].indices[1] = _ij2[1];
11244 vinfos[2].maxsolutions = _nj2;
11245 vinfos[3].jointtype = 1;
11246 vinfos[3].foffset = j3;
11247 vinfos[3].indices[0] = _ij3[0];
11248 vinfos[3].indices[1] = _ij3[1];
11249 vinfos[3].maxsolutions = _nj3;
11250 vinfos[4].jointtype = 1;
11251 vinfos[4].foffset = j4;
11252 vinfos[4].indices[0] = _ij4[0];
11253 vinfos[4].indices[1] = _ij4[1];
11254 vinfos[4].maxsolutions = _nj4;
11255 vinfos[5].jointtype = 1;
11256 vinfos[5].foffset = j5;
11257 vinfos[5].indices[0] = _ij5[0];
11258 vinfos[5].indices[1] = _ij5[1];
11259 vinfos[5].maxsolutions = _nj5;
11260 vinfos[6].jointtype = 1;
11261 vinfos[6].foffset = j6;
11262 vinfos[6].indices[0] = _ij6[0];
11263 vinfos[6].indices[1] = _ij6[1];
11264 vinfos[6].maxsolutions = _nj6;
11265 std::vector<int> vfree(0);
11273 if( bgotonextstatement )
11275 bool bgotonextstatement =
true;
11278 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
11279 evalcond[1]=new_r22;
11280 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
11282 bgotonextstatement=
false;
11284 IkReal j6array[1], cj6array[1], sj6array[1];
11285 bool j6valid[1]={
false};
11289 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
11290 sj6array[0]=
IKsin(j6array[0]);
11291 cj6array[0]=
IKcos(j6array[0]);
11292 if( j6array[0] >
IKPI )
11296 else if( j6array[0] < -
IKPI )
11297 { j6array[0]+=
IK2PI;
11300 for(
int ij6 = 0; ij6 < 1; ++ij6)
11302 if( !j6valid[ij6] )
11306 _ij6[0] = ij6; _ij6[1] = -1;
11307 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11311 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11314 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11316 IkReal evalcond[8];
11317 IkReal x516=
IKcos(j6);
11318 IkReal x517=
IKsin(j6);
11319 IkReal x518=((1.0)*sj4);
11320 IkReal x519=((1.0)*x517);
11321 IkReal x520=((1.0)*x516);
11322 evalcond[0]=(x516+new_r20);
11323 evalcond[1]=(new_r21+(((-1.0)*x519)));
11324 evalcond[2]=(((sj4*x516))+new_r01);
11325 evalcond[3]=(((sj4*x517))+new_r00);
11326 evalcond[4]=(new_r11+(((-1.0)*cj4*x520)));
11327 evalcond[5]=((((-1.0)*cj4*x519))+new_r10);
11328 evalcond[6]=(((cj4*new_r10))+(((-1.0)*new_r00*x518))+(((-1.0)*x519)));
11329 evalcond[7]=(((cj4*new_r11))+(((-1.0)*x520))+(((-1.0)*new_r01*x518)));
11337 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11338 vinfos[0].jointtype = 17;
11339 vinfos[0].foffset = j0;
11340 vinfos[0].indices[0] = _ij0[0];
11341 vinfos[0].indices[1] = _ij0[1];
11342 vinfos[0].maxsolutions = _nj0;
11343 vinfos[1].jointtype = 1;
11344 vinfos[1].foffset = j1;
11345 vinfos[1].indices[0] = _ij1[0];
11346 vinfos[1].indices[1] = _ij1[1];
11347 vinfos[1].maxsolutions = _nj1;
11348 vinfos[2].jointtype = 1;
11349 vinfos[2].foffset = j2;
11350 vinfos[2].indices[0] = _ij2[0];
11351 vinfos[2].indices[1] = _ij2[1];
11352 vinfos[2].maxsolutions = _nj2;
11353 vinfos[3].jointtype = 1;
11354 vinfos[3].foffset = j3;
11355 vinfos[3].indices[0] = _ij3[0];
11356 vinfos[3].indices[1] = _ij3[1];
11357 vinfos[3].maxsolutions = _nj3;
11358 vinfos[4].jointtype = 1;
11359 vinfos[4].foffset = j4;
11360 vinfos[4].indices[0] = _ij4[0];
11361 vinfos[4].indices[1] = _ij4[1];
11362 vinfos[4].maxsolutions = _nj4;
11363 vinfos[5].jointtype = 1;
11364 vinfos[5].foffset = j5;
11365 vinfos[5].indices[0] = _ij5[0];
11366 vinfos[5].indices[1] = _ij5[1];
11367 vinfos[5].maxsolutions = _nj5;
11368 vinfos[6].jointtype = 1;
11369 vinfos[6].foffset = j6;
11370 vinfos[6].indices[0] = _ij6[0];
11371 vinfos[6].indices[1] = _ij6[1];
11372 vinfos[6].maxsolutions = _nj6;
11373 std::vector<int> vfree(0);
11381 if( bgotonextstatement )
11383 bool bgotonextstatement =
true;
11386 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
11387 evalcond[1]=new_r22;
11388 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
11390 bgotonextstatement=
false;
11392 IkReal j6array[1], cj6array[1], sj6array[1];
11393 bool j6valid[1]={
false};
11397 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
11398 sj6array[0]=
IKsin(j6array[0]);
11399 cj6array[0]=
IKcos(j6array[0]);
11400 if( j6array[0] >
IKPI )
11404 else if( j6array[0] < -
IKPI )
11405 { j6array[0]+=
IK2PI;
11408 for(
int ij6 = 0; ij6 < 1; ++ij6)
11410 if( !j6valid[ij6] )
11414 _ij6[0] = ij6; _ij6[1] = -1;
11415 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11419 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11422 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11424 IkReal evalcond[8];
11425 IkReal x521=
IKcos(j6);
11426 IkReal x522=
IKsin(j6);
11427 IkReal x523=((1.0)*sj4);
11428 IkReal x524=((1.0)*x521);
11429 IkReal x525=((1.0)*x522);
11430 evalcond[0]=(x522+new_r21);
11431 evalcond[1]=((((-1.0)*x524))+new_r20);
11432 evalcond[2]=(new_r01+((sj4*x521)));
11433 evalcond[3]=(new_r00+((sj4*x522)));
11434 evalcond[4]=(new_r11+(((-1.0)*cj4*x524)));
11435 evalcond[5]=(new_r10+(((-1.0)*cj4*x525)));
11436 evalcond[6]=(((cj4*new_r10))+(((-1.0)*new_r00*x523))+(((-1.0)*x525)));
11437 evalcond[7]=(((cj4*new_r11))+(((-1.0)*x524))+(((-1.0)*new_r01*x523)));
11445 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11446 vinfos[0].jointtype = 17;
11447 vinfos[0].foffset = j0;
11448 vinfos[0].indices[0] = _ij0[0];
11449 vinfos[0].indices[1] = _ij0[1];
11450 vinfos[0].maxsolutions = _nj0;
11451 vinfos[1].jointtype = 1;
11452 vinfos[1].foffset = j1;
11453 vinfos[1].indices[0] = _ij1[0];
11454 vinfos[1].indices[1] = _ij1[1];
11455 vinfos[1].maxsolutions = _nj1;
11456 vinfos[2].jointtype = 1;
11457 vinfos[2].foffset = j2;
11458 vinfos[2].indices[0] = _ij2[0];
11459 vinfos[2].indices[1] = _ij2[1];
11460 vinfos[2].maxsolutions = _nj2;
11461 vinfos[3].jointtype = 1;
11462 vinfos[3].foffset = j3;
11463 vinfos[3].indices[0] = _ij3[0];
11464 vinfos[3].indices[1] = _ij3[1];
11465 vinfos[3].maxsolutions = _nj3;
11466 vinfos[4].jointtype = 1;
11467 vinfos[4].foffset = j4;
11468 vinfos[4].indices[0] = _ij4[0];
11469 vinfos[4].indices[1] = _ij4[1];
11470 vinfos[4].maxsolutions = _nj4;
11471 vinfos[5].jointtype = 1;
11472 vinfos[5].foffset = j5;
11473 vinfos[5].indices[0] = _ij5[0];
11474 vinfos[5].indices[1] = _ij5[1];
11475 vinfos[5].maxsolutions = _nj5;
11476 vinfos[6].jointtype = 1;
11477 vinfos[6].foffset = j6;
11478 vinfos[6].indices[0] = _ij6[0];
11479 vinfos[6].indices[1] = _ij6[1];
11480 vinfos[6].maxsolutions = _nj6;
11481 std::vector<int> vfree(0);
11489 if( bgotonextstatement )
11491 bool bgotonextstatement =
true;
11494 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j5))), 6.28318530717959)));
11495 evalcond[1]=new_r20;
11496 evalcond[2]=new_r02;
11497 evalcond[3]=new_r12;
11498 evalcond[4]=new_r21;
11499 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
11501 bgotonextstatement=
false;
11503 IkReal j6array[1], cj6array[1], sj6array[1];
11504 bool j6valid[1]={
false};
11506 IkReal x526=((1.0)*new_r01);
11509 j6array[0]=
IKatan2(((((-1.0)*cj4*x526))+(((-1.0)*new_r00*sj4))), (((cj4*new_r00))+(((-1.0)*sj4*x526))));
11510 sj6array[0]=
IKsin(j6array[0]);
11511 cj6array[0]=
IKcos(j6array[0]);
11512 if( j6array[0] >
IKPI )
11516 else if( j6array[0] < -
IKPI )
11517 { j6array[0]+=
IK2PI;
11520 for(
int ij6 = 0; ij6 < 1; ++ij6)
11522 if( !j6valid[ij6] )
11526 _ij6[0] = ij6; _ij6[1] = -1;
11527 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11531 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11534 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11536 IkReal evalcond[8];
11537 IkReal x527=
IKsin(j6);
11538 IkReal x528=
IKcos(j6);
11539 IkReal x529=((1.0)*sj4);
11540 IkReal x530=((1.0)*x528);
11541 IkReal x531=(sj4*x527);
11542 IkReal x532=(sj4*x528);
11543 IkReal x533=(cj4*x527);
11544 IkReal x534=(cj4*x530);
11545 evalcond[0]=(((cj4*new_r01))+((new_r11*sj4))+x527);
11546 evalcond[1]=(x533+x532+new_r01);
11547 evalcond[2]=(((cj4*new_r00))+((new_r10*sj4))+(((-1.0)*x530)));
11548 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x527))+(((-1.0)*new_r00*x529)));
11549 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x530))+(((-1.0)*new_r01*x529)));
11550 evalcond[5]=((((-1.0)*x534))+x531+new_r00);
11551 evalcond[6]=((((-1.0)*x534))+x531+new_r11);
11552 evalcond[7]=((((-1.0)*x533))+(((-1.0)*x528*x529))+new_r10);
11560 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11561 vinfos[0].jointtype = 17;
11562 vinfos[0].foffset = j0;
11563 vinfos[0].indices[0] = _ij0[0];
11564 vinfos[0].indices[1] = _ij0[1];
11565 vinfos[0].maxsolutions = _nj0;
11566 vinfos[1].jointtype = 1;
11567 vinfos[1].foffset = j1;
11568 vinfos[1].indices[0] = _ij1[0];
11569 vinfos[1].indices[1] = _ij1[1];
11570 vinfos[1].maxsolutions = _nj1;
11571 vinfos[2].jointtype = 1;
11572 vinfos[2].foffset = j2;
11573 vinfos[2].indices[0] = _ij2[0];
11574 vinfos[2].indices[1] = _ij2[1];
11575 vinfos[2].maxsolutions = _nj2;
11576 vinfos[3].jointtype = 1;
11577 vinfos[3].foffset = j3;
11578 vinfos[3].indices[0] = _ij3[0];
11579 vinfos[3].indices[1] = _ij3[1];
11580 vinfos[3].maxsolutions = _nj3;
11581 vinfos[4].jointtype = 1;
11582 vinfos[4].foffset = j4;
11583 vinfos[4].indices[0] = _ij4[0];
11584 vinfos[4].indices[1] = _ij4[1];
11585 vinfos[4].maxsolutions = _nj4;
11586 vinfos[5].jointtype = 1;
11587 vinfos[5].foffset = j5;
11588 vinfos[5].indices[0] = _ij5[0];
11589 vinfos[5].indices[1] = _ij5[1];
11590 vinfos[5].maxsolutions = _nj5;
11591 vinfos[6].jointtype = 1;
11592 vinfos[6].foffset = j6;
11593 vinfos[6].indices[0] = _ij6[0];
11594 vinfos[6].indices[1] = _ij6[1];
11595 vinfos[6].maxsolutions = _nj6;
11596 std::vector<int> vfree(0);
11604 if( bgotonextstatement )
11606 bool bgotonextstatement =
true;
11609 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
11610 evalcond[1]=new_r20;
11611 evalcond[2]=new_r02;
11612 evalcond[3]=new_r12;
11613 evalcond[4]=new_r21;
11614 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 &&
IKabs(evalcond[4]) < 0.0000050000000000 )
11616 bgotonextstatement=
false;
11618 IkReal j6array[1], cj6array[1], sj6array[1];
11619 bool j6valid[1]={
false};
11621 IkReal x535=((1.0)*new_r00);
11624 j6array[0]=
IKatan2((((cj4*new_r01))+(((-1.0)*sj4*x535))), ((((-1.0)*new_r01*sj4))+(((-1.0)*cj4*x535))));
11625 sj6array[0]=
IKsin(j6array[0]);
11626 cj6array[0]=
IKcos(j6array[0]);
11627 if( j6array[0] >
IKPI )
11631 else if( j6array[0] < -
IKPI )
11632 { j6array[0]+=
IK2PI;
11635 for(
int ij6 = 0; ij6 < 1; ++ij6)
11637 if( !j6valid[ij6] )
11641 _ij6[0] = ij6; _ij6[1] = -1;
11642 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11646 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11649 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11651 IkReal evalcond[8];
11652 IkReal x536=
IKcos(j6);
11653 IkReal x537=
IKsin(j6);
11654 IkReal x538=((1.0)*sj4);
11655 IkReal x539=((1.0)*x537);
11656 IkReal x540=(sj4*x536);
11657 IkReal x541=((1.0)*x536);
11658 IkReal x542=(cj4*x539);
11659 evalcond[0]=(((cj4*new_r00))+((new_r10*sj4))+x536);
11660 evalcond[1]=(((cj4*new_r01))+((new_r11*sj4))+(((-1.0)*x539)));
11661 evalcond[2]=(((cj4*x536))+new_r00+((sj4*x537)));
11662 evalcond[3]=(((cj4*new_r10))+(((-1.0)*new_r00*x538))+(((-1.0)*x539)));
11663 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x541))+(((-1.0)*new_r01*x538)));
11664 evalcond[5]=((((-1.0)*x542))+x540+new_r01);
11665 evalcond[6]=((((-1.0)*x542))+x540+new_r10);
11666 evalcond[7]=((((-1.0)*x537*x538))+new_r11+(((-1.0)*cj4*x541)));
11674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11675 vinfos[0].jointtype = 17;
11676 vinfos[0].foffset = j0;
11677 vinfos[0].indices[0] = _ij0[0];
11678 vinfos[0].indices[1] = _ij0[1];
11679 vinfos[0].maxsolutions = _nj0;
11680 vinfos[1].jointtype = 1;
11681 vinfos[1].foffset = j1;
11682 vinfos[1].indices[0] = _ij1[0];
11683 vinfos[1].indices[1] = _ij1[1];
11684 vinfos[1].maxsolutions = _nj1;
11685 vinfos[2].jointtype = 1;
11686 vinfos[2].foffset = j2;
11687 vinfos[2].indices[0] = _ij2[0];
11688 vinfos[2].indices[1] = _ij2[1];
11689 vinfos[2].maxsolutions = _nj2;
11690 vinfos[3].jointtype = 1;
11691 vinfos[3].foffset = j3;
11692 vinfos[3].indices[0] = _ij3[0];
11693 vinfos[3].indices[1] = _ij3[1];
11694 vinfos[3].maxsolutions = _nj3;
11695 vinfos[4].jointtype = 1;
11696 vinfos[4].foffset = j4;
11697 vinfos[4].indices[0] = _ij4[0];
11698 vinfos[4].indices[1] = _ij4[1];
11699 vinfos[4].maxsolutions = _nj4;
11700 vinfos[5].jointtype = 1;
11701 vinfos[5].foffset = j5;
11702 vinfos[5].indices[0] = _ij5[0];
11703 vinfos[5].indices[1] = _ij5[1];
11704 vinfos[5].maxsolutions = _nj5;
11705 vinfos[6].jointtype = 1;
11706 vinfos[6].foffset = j6;
11707 vinfos[6].indices[0] = _ij6[0];
11708 vinfos[6].indices[1] = _ij6[1];
11709 vinfos[6].maxsolutions = _nj6;
11710 std::vector<int> vfree(0);
11718 if( bgotonextstatement )
11720 bool bgotonextstatement =
true;
11723 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(j4))), 6.28318530717959)));
11724 evalcond[1]=new_r12;
11725 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
11727 bgotonextstatement=
false;
11729 IkReal j6array[1], cj6array[1], sj6array[1];
11730 bool j6valid[1]={
false};
11734 j6array[0]=
IKatan2(new_r10, new_r11);
11735 sj6array[0]=
IKsin(j6array[0]);
11736 cj6array[0]=
IKcos(j6array[0]);
11737 if( j6array[0] >
IKPI )
11741 else if( j6array[0] < -
IKPI )
11742 { j6array[0]+=
IK2PI;
11745 for(
int ij6 = 0; ij6 < 1; ++ij6)
11747 if( !j6valid[ij6] )
11751 _ij6[0] = ij6; _ij6[1] = -1;
11752 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11756 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11759 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11761 IkReal evalcond[8];
11762 IkReal x543=
IKcos(j6);
11763 IkReal x544=
IKsin(j6);
11764 IkReal x545=((1.0)*cj5);
11765 IkReal x546=((1.0)*x544);
11766 IkReal x547=((1.0)*x543);
11767 evalcond[0]=(((new_r02*x543))+new_r20);
11768 evalcond[1]=((((-1.0)*x546))+new_r10);
11769 evalcond[2]=((((-1.0)*x547))+new_r11);
11770 evalcond[3]=(((cj5*x544))+new_r01);
11771 evalcond[4]=(new_r21+(((-1.0)*new_r02*x546)));
11772 evalcond[5]=(new_r00+(((-1.0)*x543*x545)));
11773 evalcond[6]=(((new_r20*sj5))+(((-1.0)*new_r00*x545))+x543);
11774 evalcond[7]=((((-1.0)*new_r01*x545))+(((-1.0)*x546))+((new_r21*sj5)));
11782 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11783 vinfos[0].jointtype = 17;
11784 vinfos[0].foffset = j0;
11785 vinfos[0].indices[0] = _ij0[0];
11786 vinfos[0].indices[1] = _ij0[1];
11787 vinfos[0].maxsolutions = _nj0;
11788 vinfos[1].jointtype = 1;
11789 vinfos[1].foffset = j1;
11790 vinfos[1].indices[0] = _ij1[0];
11791 vinfos[1].indices[1] = _ij1[1];
11792 vinfos[1].maxsolutions = _nj1;
11793 vinfos[2].jointtype = 1;
11794 vinfos[2].foffset = j2;
11795 vinfos[2].indices[0] = _ij2[0];
11796 vinfos[2].indices[1] = _ij2[1];
11797 vinfos[2].maxsolutions = _nj2;
11798 vinfos[3].jointtype = 1;
11799 vinfos[3].foffset = j3;
11800 vinfos[3].indices[0] = _ij3[0];
11801 vinfos[3].indices[1] = _ij3[1];
11802 vinfos[3].maxsolutions = _nj3;
11803 vinfos[4].jointtype = 1;
11804 vinfos[4].foffset = j4;
11805 vinfos[4].indices[0] = _ij4[0];
11806 vinfos[4].indices[1] = _ij4[1];
11807 vinfos[4].maxsolutions = _nj4;
11808 vinfos[5].jointtype = 1;
11809 vinfos[5].foffset = j5;
11810 vinfos[5].indices[0] = _ij5[0];
11811 vinfos[5].indices[1] = _ij5[1];
11812 vinfos[5].maxsolutions = _nj5;
11813 vinfos[6].jointtype = 1;
11814 vinfos[6].foffset = j6;
11815 vinfos[6].indices[0] = _ij6[0];
11816 vinfos[6].indices[1] = _ij6[1];
11817 vinfos[6].maxsolutions = _nj6;
11818 std::vector<int> vfree(0);
11826 if( bgotonextstatement )
11828 bool bgotonextstatement =
true;
11831 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
11832 evalcond[1]=new_r12;
11833 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 )
11835 bgotonextstatement=
false;
11840 j4=3.14159265358979;
11842 j6eval[1]=
IKsign(new_r02);
11843 j6eval[2]=((
IKabs(new_r20))+(
IKabs(new_r21)));
11844 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 ||
IKabs(j6eval[2]) < 0.0000010000000000 )
11850 j4=3.14159265358979;
11852 if(
IKabs(j6eval[0]) < 0.0000010000000000 )
11858 j4=3.14159265358979;
11861 if(
IKabs(j6eval[0]) < 0.0000010000000000 ||
IKabs(j6eval[1]) < 0.0000010000000000 )
11864 IkReal evalcond[4];
11865 bool bgotonextstatement =
true;
11868 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
11869 evalcond[1]=new_r22;
11870 evalcond[2]=new_r01;
11871 evalcond[3]=new_r00;
11872 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
11874 bgotonextstatement=
false;
11876 IkReal j6array[1], cj6array[1], sj6array[1];
11877 bool j6valid[1]={
false};
11881 j6array[0]=
IKatan2(new_r21, ((-1.0)*new_r20));
11882 sj6array[0]=
IKsin(j6array[0]);
11883 cj6array[0]=
IKcos(j6array[0]);
11884 if( j6array[0] >
IKPI )
11888 else if( j6array[0] < -
IKPI )
11889 { j6array[0]+=
IK2PI;
11892 for(
int ij6 = 0; ij6 < 1; ++ij6)
11894 if( !j6valid[ij6] )
11898 _ij6[0] = ij6; _ij6[1] = -1;
11899 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
11903 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
11906 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
11908 IkReal evalcond[4];
11909 IkReal x548=
IKcos(j6);
11910 IkReal x549=((1.0)*(
IKsin(j6)));
11911 evalcond[0]=(x548+new_r20);
11912 evalcond[1]=((((-1.0)*x549))+new_r21);
11913 evalcond[2]=((((-1.0)*x549))+(((-1.0)*new_r10)));
11914 evalcond[3]=((((-1.0)*x548))+(((-1.0)*new_r11)));
11922 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
11923 vinfos[0].jointtype = 17;
11924 vinfos[0].foffset = j0;
11925 vinfos[0].indices[0] = _ij0[0];
11926 vinfos[0].indices[1] = _ij0[1];
11927 vinfos[0].maxsolutions = _nj0;
11928 vinfos[1].jointtype = 1;
11929 vinfos[1].foffset = j1;
11930 vinfos[1].indices[0] = _ij1[0];
11931 vinfos[1].indices[1] = _ij1[1];
11932 vinfos[1].maxsolutions = _nj1;
11933 vinfos[2].jointtype = 1;
11934 vinfos[2].foffset = j2;
11935 vinfos[2].indices[0] = _ij2[0];
11936 vinfos[2].indices[1] = _ij2[1];
11937 vinfos[2].maxsolutions = _nj2;
11938 vinfos[3].jointtype = 1;
11939 vinfos[3].foffset = j3;
11940 vinfos[3].indices[0] = _ij3[0];
11941 vinfos[3].indices[1] = _ij3[1];
11942 vinfos[3].maxsolutions = _nj3;
11943 vinfos[4].jointtype = 1;
11944 vinfos[4].foffset = j4;
11945 vinfos[4].indices[0] = _ij4[0];
11946 vinfos[4].indices[1] = _ij4[1];
11947 vinfos[4].maxsolutions = _nj4;
11948 vinfos[5].jointtype = 1;
11949 vinfos[5].foffset = j5;
11950 vinfos[5].indices[0] = _ij5[0];
11951 vinfos[5].indices[1] = _ij5[1];
11952 vinfos[5].maxsolutions = _nj5;
11953 vinfos[6].jointtype = 1;
11954 vinfos[6].foffset = j6;
11955 vinfos[6].indices[0] = _ij6[0];
11956 vinfos[6].indices[1] = _ij6[1];
11957 vinfos[6].maxsolutions = _nj6;
11958 std::vector<int> vfree(0);
11966 if( bgotonextstatement )
11968 bool bgotonextstatement =
true;
11971 evalcond[0]=((-3.14159265358979)+(
IKfmod(((3.14159265358979)+(
IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
11972 evalcond[1]=new_r22;
11973 evalcond[2]=new_r01;
11974 evalcond[3]=new_r00;
11975 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 &&
IKabs(evalcond[3]) < 0.0000050000000000 )
11977 bgotonextstatement=
false;
11979 IkReal j6array[1], cj6array[1], sj6array[1];
11980 bool j6valid[1]={
false};
11984 j6array[0]=
IKatan2(((-1.0)*new_r21), new_r20);
11985 sj6array[0]=
IKsin(j6array[0]);
11986 cj6array[0]=
IKcos(j6array[0]);
11987 if( j6array[0] >
IKPI )
11991 else if( j6array[0] < -
IKPI )
11992 { j6array[0]+=
IK2PI;
11995 for(
int ij6 = 0; ij6 < 1; ++ij6)
11997 if( !j6valid[ij6] )
12001 _ij6[0] = ij6; _ij6[1] = -1;
12002 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12006 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12009 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12011 IkReal evalcond[4];
12012 IkReal x550=
IKsin(j6);
12013 IkReal x551=((1.0)*(
IKcos(j6)));
12014 evalcond[0]=(x550+new_r21);
12015 evalcond[1]=((((-1.0)*x551))+new_r20);
12016 evalcond[2]=((((-1.0)*x550))+(((-1.0)*new_r10)));
12017 evalcond[3]=((((-1.0)*x551))+(((-1.0)*new_r11)));
12025 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12026 vinfos[0].jointtype = 17;
12027 vinfos[0].foffset = j0;
12028 vinfos[0].indices[0] = _ij0[0];
12029 vinfos[0].indices[1] = _ij0[1];
12030 vinfos[0].maxsolutions = _nj0;
12031 vinfos[1].jointtype = 1;
12032 vinfos[1].foffset = j1;
12033 vinfos[1].indices[0] = _ij1[0];
12034 vinfos[1].indices[1] = _ij1[1];
12035 vinfos[1].maxsolutions = _nj1;
12036 vinfos[2].jointtype = 1;
12037 vinfos[2].foffset = j2;
12038 vinfos[2].indices[0] = _ij2[0];
12039 vinfos[2].indices[1] = _ij2[1];
12040 vinfos[2].maxsolutions = _nj2;
12041 vinfos[3].jointtype = 1;
12042 vinfos[3].foffset = j3;
12043 vinfos[3].indices[0] = _ij3[0];
12044 vinfos[3].indices[1] = _ij3[1];
12045 vinfos[3].maxsolutions = _nj3;
12046 vinfos[4].jointtype = 1;
12047 vinfos[4].foffset = j4;
12048 vinfos[4].indices[0] = _ij4[0];
12049 vinfos[4].indices[1] = _ij4[1];
12050 vinfos[4].maxsolutions = _nj4;
12051 vinfos[5].jointtype = 1;
12052 vinfos[5].foffset = j5;
12053 vinfos[5].indices[0] = _ij5[0];
12054 vinfos[5].indices[1] = _ij5[1];
12055 vinfos[5].maxsolutions = _nj5;
12056 vinfos[6].jointtype = 1;
12057 vinfos[6].foffset = j6;
12058 vinfos[6].indices[0] = _ij6[0];
12059 vinfos[6].indices[1] = _ij6[1];
12060 vinfos[6].maxsolutions = _nj6;
12061 std::vector<int> vfree(0);
12069 if( bgotonextstatement )
12071 bool bgotonextstatement =
true;
12074 evalcond[0]=
IKabs(new_r02);
12075 evalcond[1]=new_r20;
12076 evalcond[2]=new_r21;
12077 if(
IKabs(evalcond[0]) < 0.0000050000000000 &&
IKabs(evalcond[1]) < 0.0000050000000000 &&
IKabs(evalcond[2]) < 0.0000050000000000 )
12079 bgotonextstatement=
false;
12081 IkReal j6array[1], cj6array[1], sj6array[1];
12082 bool j6valid[1]={
false};
12086 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*cj5*new_r00));
12087 sj6array[0]=
IKsin(j6array[0]);
12088 cj6array[0]=
IKcos(j6array[0]);
12089 if( j6array[0] >
IKPI )
12093 else if( j6array[0] < -
IKPI )
12094 { j6array[0]+=
IK2PI;
12097 for(
int ij6 = 0; ij6 < 1; ++ij6)
12099 if( !j6valid[ij6] )
12103 _ij6[0] = ij6; _ij6[1] = -1;
12104 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12108 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12111 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12113 IkReal evalcond[6];
12114 IkReal x552=
IKcos(j6);
12115 IkReal x553=
IKsin(j6);
12116 IkReal x554=((1.0)*x553);
12117 IkReal x555=((1.0)*x552);
12118 evalcond[0]=(((cj5*new_r00))+x552);
12119 evalcond[1]=((((-1.0)*x554))+(((-1.0)*new_r10)));
12120 evalcond[2]=((((-1.0)*x555))+(((-1.0)*new_r11)));
12121 evalcond[3]=(((cj5*x553))+(((-1.0)*new_r01)));
12122 evalcond[4]=(((cj5*new_r01))+(((-1.0)*x554)));
12123 evalcond[5]=((((-1.0)*cj5*x555))+(((-1.0)*new_r00)));
12131 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12132 vinfos[0].jointtype = 17;
12133 vinfos[0].foffset = j0;
12134 vinfos[0].indices[0] = _ij0[0];
12135 vinfos[0].indices[1] = _ij0[1];
12136 vinfos[0].maxsolutions = _nj0;
12137 vinfos[1].jointtype = 1;
12138 vinfos[1].foffset = j1;
12139 vinfos[1].indices[0] = _ij1[0];
12140 vinfos[1].indices[1] = _ij1[1];
12141 vinfos[1].maxsolutions = _nj1;
12142 vinfos[2].jointtype = 1;
12143 vinfos[2].foffset = j2;
12144 vinfos[2].indices[0] = _ij2[0];
12145 vinfos[2].indices[1] = _ij2[1];
12146 vinfos[2].maxsolutions = _nj2;
12147 vinfos[3].jointtype = 1;
12148 vinfos[3].foffset = j3;
12149 vinfos[3].indices[0] = _ij3[0];
12150 vinfos[3].indices[1] = _ij3[1];
12151 vinfos[3].maxsolutions = _nj3;
12152 vinfos[4].jointtype = 1;
12153 vinfos[4].foffset = j4;
12154 vinfos[4].indices[0] = _ij4[0];
12155 vinfos[4].indices[1] = _ij4[1];
12156 vinfos[4].maxsolutions = _nj4;
12157 vinfos[5].jointtype = 1;
12158 vinfos[5].foffset = j5;
12159 vinfos[5].indices[0] = _ij5[0];
12160 vinfos[5].indices[1] = _ij5[1];
12161 vinfos[5].maxsolutions = _nj5;
12162 vinfos[6].jointtype = 1;
12163 vinfos[6].foffset = j6;
12164 vinfos[6].indices[0] = _ij6[0];
12165 vinfos[6].indices[1] = _ij6[1];
12166 vinfos[6].maxsolutions = _nj6;
12167 std::vector<int> vfree(0);
12175 if( bgotonextstatement )
12177 bool bgotonextstatement =
true;
12180 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
12181 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
12183 bgotonextstatement=
false;
12185 IkReal j6array[1], cj6array[1], sj6array[1];
12186 bool j6valid[1]={
false};
12190 j6array[0]=
IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
12191 sj6array[0]=
IKsin(j6array[0]);
12192 cj6array[0]=
IKcos(j6array[0]);
12193 if( j6array[0] >
IKPI )
12197 else if( j6array[0] < -
IKPI )
12198 { j6array[0]+=
IK2PI;
12201 for(
int ij6 = 0; ij6 < 1; ++ij6)
12203 if( !j6valid[ij6] )
12207 _ij6[0] = ij6; _ij6[1] = -1;
12208 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12212 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12215 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12217 IkReal evalcond[6];
12218 IkReal x556=
IKsin(j6);
12219 IkReal x557=
IKcos(j6);
12221 evalcond[1]=(new_r22*x556);
12222 evalcond[2]=((-1.0)*x556);
12223 evalcond[3]=((-1.0)*new_r22*x557);
12224 evalcond[4]=((((-1.0)*x556))+(((-1.0)*new_r10)));
12225 evalcond[5]=((((-1.0)*x557))+(((-1.0)*new_r11)));
12233 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12234 vinfos[0].jointtype = 17;
12235 vinfos[0].foffset = j0;
12236 vinfos[0].indices[0] = _ij0[0];
12237 vinfos[0].indices[1] = _ij0[1];
12238 vinfos[0].maxsolutions = _nj0;
12239 vinfos[1].jointtype = 1;
12240 vinfos[1].foffset = j1;
12241 vinfos[1].indices[0] = _ij1[0];
12242 vinfos[1].indices[1] = _ij1[1];
12243 vinfos[1].maxsolutions = _nj1;
12244 vinfos[2].jointtype = 1;
12245 vinfos[2].foffset = j2;
12246 vinfos[2].indices[0] = _ij2[0];
12247 vinfos[2].indices[1] = _ij2[1];
12248 vinfos[2].maxsolutions = _nj2;
12249 vinfos[3].jointtype = 1;
12250 vinfos[3].foffset = j3;
12251 vinfos[3].indices[0] = _ij3[0];
12252 vinfos[3].indices[1] = _ij3[1];
12253 vinfos[3].maxsolutions = _nj3;
12254 vinfos[4].jointtype = 1;
12255 vinfos[4].foffset = j4;
12256 vinfos[4].indices[0] = _ij4[0];
12257 vinfos[4].indices[1] = _ij4[1];
12258 vinfos[4].maxsolutions = _nj4;
12259 vinfos[5].jointtype = 1;
12260 vinfos[5].foffset = j5;
12261 vinfos[5].indices[0] = _ij5[0];
12262 vinfos[5].indices[1] = _ij5[1];
12263 vinfos[5].maxsolutions = _nj5;
12264 vinfos[6].jointtype = 1;
12265 vinfos[6].foffset = j6;
12266 vinfos[6].indices[0] = _ij6[0];
12267 vinfos[6].indices[1] = _ij6[1];
12268 vinfos[6].maxsolutions = _nj6;
12269 std::vector<int> vfree(0);
12277 if( bgotonextstatement )
12279 bool bgotonextstatement =
true;
12284 bgotonextstatement=
false;
12289 if( bgotonextstatement )
12301 IkReal j6array[1], cj6array[1], sj6array[1];
12302 bool j6valid[1]={
false};
12314 j6array[0]=
IKatan2(((-1.0)*new_r21*(x558.
value)), ((-1.0)*new_r00*(x559.
value)));
12315 sj6array[0]=
IKsin(j6array[0]);
12316 cj6array[0]=
IKcos(j6array[0]);
12317 if( j6array[0] >
IKPI )
12321 else if( j6array[0] < -
IKPI )
12322 { j6array[0]+=
IK2PI;
12325 for(
int ij6 = 0; ij6 < 1; ++ij6)
12327 if( !j6valid[ij6] )
12331 _ij6[0] = ij6; _ij6[1] = -1;
12332 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12336 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12339 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12341 IkReal evalcond[8];
12342 IkReal x560=
IKsin(j6);
12343 IkReal x561=
IKcos(j6);
12344 IkReal x562=((1.0)*x560);
12345 IkReal x563=((1.0)*x561);
12346 evalcond[0]=(((new_r02*x560))+new_r21);
12347 evalcond[1]=((((-1.0)*new_r02*x563))+new_r20);
12348 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x562)));
12349 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x563)));
12350 evalcond[4]=((((-1.0)*new_r01))+((cj5*x560)));
12351 evalcond[5]=((((-1.0)*cj5*x563))+(((-1.0)*new_r00)));
12352 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+x561);
12353 evalcond[7]=(((cj5*new_r01))+((new_r21*sj5))+(((-1.0)*x562)));
12361 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12362 vinfos[0].jointtype = 17;
12363 vinfos[0].foffset = j0;
12364 vinfos[0].indices[0] = _ij0[0];
12365 vinfos[0].indices[1] = _ij0[1];
12366 vinfos[0].maxsolutions = _nj0;
12367 vinfos[1].jointtype = 1;
12368 vinfos[1].foffset = j1;
12369 vinfos[1].indices[0] = _ij1[0];
12370 vinfos[1].indices[1] = _ij1[1];
12371 vinfos[1].maxsolutions = _nj1;
12372 vinfos[2].jointtype = 1;
12373 vinfos[2].foffset = j2;
12374 vinfos[2].indices[0] = _ij2[0];
12375 vinfos[2].indices[1] = _ij2[1];
12376 vinfos[2].maxsolutions = _nj2;
12377 vinfos[3].jointtype = 1;
12378 vinfos[3].foffset = j3;
12379 vinfos[3].indices[0] = _ij3[0];
12380 vinfos[3].indices[1] = _ij3[1];
12381 vinfos[3].maxsolutions = _nj3;
12382 vinfos[4].jointtype = 1;
12383 vinfos[4].foffset = j4;
12384 vinfos[4].indices[0] = _ij4[0];
12385 vinfos[4].indices[1] = _ij4[1];
12386 vinfos[4].maxsolutions = _nj4;
12387 vinfos[5].jointtype = 1;
12388 vinfos[5].foffset = j5;
12389 vinfos[5].indices[0] = _ij5[0];
12390 vinfos[5].indices[1] = _ij5[1];
12391 vinfos[5].maxsolutions = _nj5;
12392 vinfos[6].jointtype = 1;
12393 vinfos[6].foffset = j6;
12394 vinfos[6].indices[0] = _ij6[0];
12395 vinfos[6].indices[1] = _ij6[1];
12396 vinfos[6].maxsolutions = _nj6;
12397 std::vector<int> vfree(0);
12410 IkReal j6array[1], cj6array[1], sj6array[1];
12411 bool j6valid[1]={
false};
12419 j6array[0]=
IKatan2(((-1.0)*new_r10), (new_r20*(x564.
value)));
12420 sj6array[0]=
IKsin(j6array[0]);
12421 cj6array[0]=
IKcos(j6array[0]);
12422 if( j6array[0] >
IKPI )
12426 else if( j6array[0] < -
IKPI )
12427 { j6array[0]+=
IK2PI;
12430 for(
int ij6 = 0; ij6 < 1; ++ij6)
12432 if( !j6valid[ij6] )
12436 _ij6[0] = ij6; _ij6[1] = -1;
12437 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12441 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12444 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12446 IkReal evalcond[8];
12447 IkReal x565=
IKsin(j6);
12448 IkReal x566=
IKcos(j6);
12449 IkReal x567=((1.0)*x565);
12450 IkReal x568=((1.0)*x566);
12451 evalcond[0]=(((new_r02*x565))+new_r21);
12452 evalcond[1]=((((-1.0)*new_r02*x568))+new_r20);
12453 evalcond[2]=((((-1.0)*new_r10))+(((-1.0)*x567)));
12454 evalcond[3]=((((-1.0)*new_r11))+(((-1.0)*x568)));
12455 evalcond[4]=((((-1.0)*new_r01))+((cj5*x565)));
12456 evalcond[5]=((((-1.0)*cj5*x568))+(((-1.0)*new_r00)));
12457 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+x566);
12458 evalcond[7]=(((cj5*new_r01))+((new_r21*sj5))+(((-1.0)*x567)));
12466 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12467 vinfos[0].jointtype = 17;
12468 vinfos[0].foffset = j0;
12469 vinfos[0].indices[0] = _ij0[0];
12470 vinfos[0].indices[1] = _ij0[1];
12471 vinfos[0].maxsolutions = _nj0;
12472 vinfos[1].jointtype = 1;
12473 vinfos[1].foffset = j1;
12474 vinfos[1].indices[0] = _ij1[0];
12475 vinfos[1].indices[1] = _ij1[1];
12476 vinfos[1].maxsolutions = _nj1;
12477 vinfos[2].jointtype = 1;
12478 vinfos[2].foffset = j2;
12479 vinfos[2].indices[0] = _ij2[0];
12480 vinfos[2].indices[1] = _ij2[1];
12481 vinfos[2].maxsolutions = _nj2;
12482 vinfos[3].jointtype = 1;
12483 vinfos[3].foffset = j3;
12484 vinfos[3].indices[0] = _ij3[0];
12485 vinfos[3].indices[1] = _ij3[1];
12486 vinfos[3].maxsolutions = _nj3;
12487 vinfos[4].jointtype = 1;
12488 vinfos[4].foffset = j4;
12489 vinfos[4].indices[0] = _ij4[0];
12490 vinfos[4].indices[1] = _ij4[1];
12491 vinfos[4].maxsolutions = _nj4;
12492 vinfos[5].jointtype = 1;
12493 vinfos[5].foffset = j5;
12494 vinfos[5].indices[0] = _ij5[0];
12495 vinfos[5].indices[1] = _ij5[1];
12496 vinfos[5].maxsolutions = _nj5;
12497 vinfos[6].jointtype = 1;
12498 vinfos[6].foffset = j6;
12499 vinfos[6].indices[0] = _ij6[0];
12500 vinfos[6].indices[1] = _ij6[1];
12501 vinfos[6].maxsolutions = _nj6;
12502 std::vector<int> vfree(0);
12515 IkReal j6array[1], cj6array[1], sj6array[1];
12516 bool j6valid[1]={
false};
12526 j6array[0]=((-1.5707963267949)+(x569.
value)+(((1.5707963267949)*(x570.
value))));
12527 sj6array[0]=
IKsin(j6array[0]);
12528 cj6array[0]=
IKcos(j6array[0]);
12529 if( j6array[0] >
IKPI )
12533 else if( j6array[0] < -
IKPI )
12534 { j6array[0]+=
IK2PI;
12537 for(
int ij6 = 0; ij6 < 1; ++ij6)
12539 if( !j6valid[ij6] )
12543 _ij6[0] = ij6; _ij6[1] = -1;
12544 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12548 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12551 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12553 IkReal evalcond[8];
12554 IkReal x571=
IKsin(j6);
12555 IkReal x572=
IKcos(j6);
12556 IkReal x573=((1.0)*x571);
12557 IkReal x574=((1.0)*x572);
12558 evalcond[0]=(new_r21+((new_r02*x571)));
12559 evalcond[1]=((((-1.0)*new_r02*x574))+new_r20);
12560 evalcond[2]=((((-1.0)*x573))+(((-1.0)*new_r10)));
12561 evalcond[3]=((((-1.0)*x574))+(((-1.0)*new_r11)));
12562 evalcond[4]=(((cj5*x571))+(((-1.0)*new_r01)));
12563 evalcond[5]=((((-1.0)*cj5*x574))+(((-1.0)*new_r00)));
12564 evalcond[6]=(((new_r20*sj5))+((cj5*new_r00))+x572);
12565 evalcond[7]=(((cj5*new_r01))+(((-1.0)*x573))+((new_r21*sj5)));
12573 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12574 vinfos[0].jointtype = 17;
12575 vinfos[0].foffset = j0;
12576 vinfos[0].indices[0] = _ij0[0];
12577 vinfos[0].indices[1] = _ij0[1];
12578 vinfos[0].maxsolutions = _nj0;
12579 vinfos[1].jointtype = 1;
12580 vinfos[1].foffset = j1;
12581 vinfos[1].indices[0] = _ij1[0];
12582 vinfos[1].indices[1] = _ij1[1];
12583 vinfos[1].maxsolutions = _nj1;
12584 vinfos[2].jointtype = 1;
12585 vinfos[2].foffset = j2;
12586 vinfos[2].indices[0] = _ij2[0];
12587 vinfos[2].indices[1] = _ij2[1];
12588 vinfos[2].maxsolutions = _nj2;
12589 vinfos[3].jointtype = 1;
12590 vinfos[3].foffset = j3;
12591 vinfos[3].indices[0] = _ij3[0];
12592 vinfos[3].indices[1] = _ij3[1];
12593 vinfos[3].maxsolutions = _nj3;
12594 vinfos[4].jointtype = 1;
12595 vinfos[4].foffset = j4;
12596 vinfos[4].indices[0] = _ij4[0];
12597 vinfos[4].indices[1] = _ij4[1];
12598 vinfos[4].maxsolutions = _nj4;
12599 vinfos[5].jointtype = 1;
12600 vinfos[5].foffset = j5;
12601 vinfos[5].indices[0] = _ij5[0];
12602 vinfos[5].indices[1] = _ij5[1];
12603 vinfos[5].maxsolutions = _nj5;
12604 vinfos[6].jointtype = 1;
12605 vinfos[6].foffset = j6;
12606 vinfos[6].indices[0] = _ij6[0];
12607 vinfos[6].indices[1] = _ij6[1];
12608 vinfos[6].maxsolutions = _nj6;
12609 std::vector<int> vfree(0);
12621 if( bgotonextstatement )
12623 bool bgotonextstatement =
true;
12626 evalcond[0]=((
IKabs(new_r20))+(
IKabs(new_r21)));
12627 if(
IKabs(evalcond[0]) < 0.0000050000000000 )
12629 bgotonextstatement=
false;
12637 if(
IKabs(j6eval[0]) < 0.0000000100000000 )
12643 IkReal op[2+1], zeror[2];
12648 polyroots2(op,zeror,numroots);
12649 IkReal j6array[2], cj6array[2], sj6array[2], tempj6array[1];
12650 int numsolutions = 0;
12651 for(
int ij6 = 0; ij6 < numroots; ++ij6)
12653 IkReal htj6 = zeror[ij6];
12654 tempj6array[0]=((2.0)*(
atan(htj6)));
12655 for(
int kj6 = 0; kj6 < 1; ++kj6)
12657 j6array[numsolutions] = tempj6array[kj6];
12658 if( j6array[numsolutions] >
IKPI )
12660 j6array[numsolutions]-=
IK2PI;
12662 else if( j6array[numsolutions] < -
IKPI )
12664 j6array[numsolutions]+=
IK2PI;
12666 sj6array[numsolutions] =
IKsin(j6array[numsolutions]);
12667 cj6array[numsolutions] =
IKcos(j6array[numsolutions]);
12671 bool j6valid[2]={
true,
true};
12673 for(
int ij6 = 0; ij6 < numsolutions; ++ij6)
12675 if( !j6valid[ij6] )
12679 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12680 htj6 =
IKtan(j6/2);
12682 _ij6[0] = ij6; _ij6[1] = -1;
12683 for(
int iij6 = ij6+1; iij6 < numsolutions; ++iij6)
12687 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12691 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12692 vinfos[0].jointtype = 17;
12693 vinfos[0].foffset = j0;
12694 vinfos[0].indices[0] = _ij0[0];
12695 vinfos[0].indices[1] = _ij0[1];
12696 vinfos[0].maxsolutions = _nj0;
12697 vinfos[1].jointtype = 1;
12698 vinfos[1].foffset = j1;
12699 vinfos[1].indices[0] = _ij1[0];
12700 vinfos[1].indices[1] = _ij1[1];
12701 vinfos[1].maxsolutions = _nj1;
12702 vinfos[2].jointtype = 1;
12703 vinfos[2].foffset = j2;
12704 vinfos[2].indices[0] = _ij2[0];
12705 vinfos[2].indices[1] = _ij2[1];
12706 vinfos[2].maxsolutions = _nj2;
12707 vinfos[3].jointtype = 1;
12708 vinfos[3].foffset = j3;
12709 vinfos[3].indices[0] = _ij3[0];
12710 vinfos[3].indices[1] = _ij3[1];
12711 vinfos[3].maxsolutions = _nj3;
12712 vinfos[4].jointtype = 1;
12713 vinfos[4].foffset = j4;
12714 vinfos[4].indices[0] = _ij4[0];
12715 vinfos[4].indices[1] = _ij4[1];
12716 vinfos[4].maxsolutions = _nj4;
12717 vinfos[5].jointtype = 1;
12718 vinfos[5].foffset = j5;
12719 vinfos[5].indices[0] = _ij5[0];
12720 vinfos[5].indices[1] = _ij5[1];
12721 vinfos[5].maxsolutions = _nj5;
12722 vinfos[6].jointtype = 1;
12723 vinfos[6].foffset = j6;
12724 vinfos[6].indices[0] = _ij6[0];
12725 vinfos[6].indices[1] = _ij6[1];
12726 vinfos[6].maxsolutions = _nj6;
12727 std::vector<int> vfree(0);
12738 if( bgotonextstatement )
12740 bool bgotonextstatement =
true;
12745 bgotonextstatement=
false;
12750 if( bgotonextstatement )
12767 IkReal j6array[1], cj6array[1], sj6array[1];
12768 bool j6valid[1]={
false};
12774 IkReal x575=x576.
value;
12785 j6array[0]=
IKatan2((x575*(x577.
value)*(x578.
value)*((((new_r20*sj4))+(((-1.0)*new_r01*sj5))))), ((-1.0)*new_r20*x575));
12786 sj6array[0]=
IKsin(j6array[0]);
12787 cj6array[0]=
IKcos(j6array[0]);
12788 if( j6array[0] >
IKPI )
12792 else if( j6array[0] < -
IKPI )
12793 { j6array[0]+=
IK2PI;
12796 for(
int ij6 = 0; ij6 < 1; ++ij6)
12798 if( !j6valid[ij6] )
12802 _ij6[0] = ij6; _ij6[1] = -1;
12803 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12807 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12810 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12812 IkReal evalcond[12];
12813 IkReal x579=
IKsin(j6);
12814 IkReal x580=
IKcos(j6);
12815 IkReal x581=((1.0)*sj4);
12816 IkReal x582=(cj4*new_r01);
12817 IkReal x583=((1.0)*cj5);
12818 IkReal x584=(cj4*new_r00);
12819 IkReal x585=((1.0)*x579);
12820 IkReal x586=(cj5*x579);
12821 IkReal x587=(cj4*x580);
12822 evalcond[0]=(((sj5*x580))+new_r20);
12823 evalcond[1]=((((-1.0)*sj5*x585))+new_r21);
12824 evalcond[2]=(((new_r11*sj4))+x582+x586);
12825 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x585))+(((-1.0)*new_r00*x581)));
12826 evalcond[4]=((((-1.0)*x580))+(((-1.0)*new_r01*x581))+((cj4*new_r11)));
12827 evalcond[5]=(((cj4*x586))+((sj4*x580))+new_r01);
12828 evalcond[6]=((((-1.0)*x580*x583))+((new_r10*sj4))+x584);
12829 evalcond[7]=(((sj4*x579))+(((-1.0)*x583*x587))+new_r00);
12830 evalcond[8]=((((-1.0)*x587))+((sj4*x586))+new_r11);
12831 evalcond[9]=((((-1.0)*cj5*x580*x581))+(((-1.0)*cj4*x585))+new_r10);
12832 evalcond[10]=((((-1.0)*cj5*new_r10*x581))+((new_r20*sj5))+(((-1.0)*x583*x584))+x580);
12833 evalcond[11]=((((-1.0)*cj5*new_r11*x581))+(((-1.0)*x582*x583))+((new_r21*sj5))+(((-1.0)*x585)));
12841 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12842 vinfos[0].jointtype = 17;
12843 vinfos[0].foffset = j0;
12844 vinfos[0].indices[0] = _ij0[0];
12845 vinfos[0].indices[1] = _ij0[1];
12846 vinfos[0].maxsolutions = _nj0;
12847 vinfos[1].jointtype = 1;
12848 vinfos[1].foffset = j1;
12849 vinfos[1].indices[0] = _ij1[0];
12850 vinfos[1].indices[1] = _ij1[1];
12851 vinfos[1].maxsolutions = _nj1;
12852 vinfos[2].jointtype = 1;
12853 vinfos[2].foffset = j2;
12854 vinfos[2].indices[0] = _ij2[0];
12855 vinfos[2].indices[1] = _ij2[1];
12856 vinfos[2].maxsolutions = _nj2;
12857 vinfos[3].jointtype = 1;
12858 vinfos[3].foffset = j3;
12859 vinfos[3].indices[0] = _ij3[0];
12860 vinfos[3].indices[1] = _ij3[1];
12861 vinfos[3].maxsolutions = _nj3;
12862 vinfos[4].jointtype = 1;
12863 vinfos[4].foffset = j4;
12864 vinfos[4].indices[0] = _ij4[0];
12865 vinfos[4].indices[1] = _ij4[1];
12866 vinfos[4].maxsolutions = _nj4;
12867 vinfos[5].jointtype = 1;
12868 vinfos[5].foffset = j5;
12869 vinfos[5].indices[0] = _ij5[0];
12870 vinfos[5].indices[1] = _ij5[1];
12871 vinfos[5].maxsolutions = _nj5;
12872 vinfos[6].jointtype = 1;
12873 vinfos[6].foffset = j6;
12874 vinfos[6].indices[0] = _ij6[0];
12875 vinfos[6].indices[1] = _ij6[1];
12876 vinfos[6].maxsolutions = _nj6;
12877 std::vector<int> vfree(0);
12890 IkReal j6array[1], cj6array[1], sj6array[1];
12891 bool j6valid[1]={
false};
12897 IkReal x588=x589.
value;
12904 j6array[0]=
IKatan2((x588*(x590.
value)*(((((-1.0)*new_r00*sj5))+(((-1.0)*cj4*cj5*new_r20))))), ((-1.0)*new_r20*x588));
12905 sj6array[0]=
IKsin(j6array[0]);
12906 cj6array[0]=
IKcos(j6array[0]);
12907 if( j6array[0] >
IKPI )
12911 else if( j6array[0] < -
IKPI )
12912 { j6array[0]+=
IK2PI;
12915 for(
int ij6 = 0; ij6 < 1; ++ij6)
12917 if( !j6valid[ij6] )
12921 _ij6[0] = ij6; _ij6[1] = -1;
12922 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
12926 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
12929 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
12931 IkReal evalcond[12];
12932 IkReal x591=
IKsin(j6);
12933 IkReal x592=
IKcos(j6);
12934 IkReal x593=((1.0)*sj4);
12935 IkReal x594=(cj4*new_r01);
12936 IkReal x595=((1.0)*cj5);
12937 IkReal x596=(cj4*new_r00);
12938 IkReal x597=((1.0)*x591);
12939 IkReal x598=(cj5*x591);
12940 IkReal x599=(cj4*x592);
12941 evalcond[0]=(new_r20+((sj5*x592)));
12942 evalcond[1]=((((-1.0)*sj5*x597))+new_r21);
12943 evalcond[2]=(((new_r11*sj4))+x594+x598);
12944 evalcond[3]=((((-1.0)*new_r00*x593))+((cj4*new_r10))+(((-1.0)*x597)));
12945 evalcond[4]=(((cj4*new_r11))+(((-1.0)*new_r01*x593))+(((-1.0)*x592)));
12946 evalcond[5]=(((sj4*x592))+new_r01+((cj4*x598)));
12947 evalcond[6]=(((new_r10*sj4))+(((-1.0)*x592*x595))+x596);
12948 evalcond[7]=(((sj4*x591))+(((-1.0)*x595*x599))+new_r00);
12949 evalcond[8]=((((-1.0)*x599))+((sj4*x598))+new_r11);
12950 evalcond[9]=((((-1.0)*cj4*x597))+new_r10+(((-1.0)*cj5*x592*x593)));
12951 evalcond[10]=(((new_r20*sj5))+x592+(((-1.0)*x595*x596))+(((-1.0)*cj5*new_r10*x593)));
12952 evalcond[11]=((((-1.0)*x597))+(((-1.0)*x594*x595))+((new_r21*sj5))+(((-1.0)*cj5*new_r11*x593)));
12960 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
12961 vinfos[0].jointtype = 17;
12962 vinfos[0].foffset = j0;
12963 vinfos[0].indices[0] = _ij0[0];
12964 vinfos[0].indices[1] = _ij0[1];
12965 vinfos[0].maxsolutions = _nj0;
12966 vinfos[1].jointtype = 1;
12967 vinfos[1].foffset = j1;
12968 vinfos[1].indices[0] = _ij1[0];
12969 vinfos[1].indices[1] = _ij1[1];
12970 vinfos[1].maxsolutions = _nj1;
12971 vinfos[2].jointtype = 1;
12972 vinfos[2].foffset = j2;
12973 vinfos[2].indices[0] = _ij2[0];
12974 vinfos[2].indices[1] = _ij2[1];
12975 vinfos[2].maxsolutions = _nj2;
12976 vinfos[3].jointtype = 1;
12977 vinfos[3].foffset = j3;
12978 vinfos[3].indices[0] = _ij3[0];
12979 vinfos[3].indices[1] = _ij3[1];
12980 vinfos[3].maxsolutions = _nj3;
12981 vinfos[4].jointtype = 1;
12982 vinfos[4].foffset = j4;
12983 vinfos[4].indices[0] = _ij4[0];
12984 vinfos[4].indices[1] = _ij4[1];
12985 vinfos[4].maxsolutions = _nj4;
12986 vinfos[5].jointtype = 1;
12987 vinfos[5].foffset = j5;
12988 vinfos[5].indices[0] = _ij5[0];
12989 vinfos[5].indices[1] = _ij5[1];
12990 vinfos[5].maxsolutions = _nj5;
12991 vinfos[6].jointtype = 1;
12992 vinfos[6].foffset = j6;
12993 vinfos[6].indices[0] = _ij6[0];
12994 vinfos[6].indices[1] = _ij6[1];
12995 vinfos[6].maxsolutions = _nj6;
12996 std::vector<int> vfree(0);
13009 IkReal j6array[1], cj6array[1], sj6array[1];
13010 bool j6valid[1]={
false};
13020 j6array[0]=((-1.5707963267949)+(((1.5707963267949)*(x600.
value)))+(x601.
value));
13021 sj6array[0]=
IKsin(j6array[0]);
13022 cj6array[0]=
IKcos(j6array[0]);
13023 if( j6array[0] >
IKPI )
13027 else if( j6array[0] < -
IKPI )
13028 { j6array[0]+=
IK2PI;
13031 for(
int ij6 = 0; ij6 < 1; ++ij6)
13033 if( !j6valid[ij6] )
13037 _ij6[0] = ij6; _ij6[1] = -1;
13038 for(
int iij6 = ij6+1; iij6 < 1; ++iij6)
13042 j6valid[iij6]=
false; _ij6[1] = iij6;
break;
13045 j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6];
13047 IkReal evalcond[12];
13048 IkReal x602=
IKsin(j6);
13049 IkReal x603=
IKcos(j6);
13050 IkReal x604=((1.0)*sj4);
13051 IkReal x605=(cj4*new_r01);
13052 IkReal x606=((1.0)*cj5);
13053 IkReal x607=(cj4*new_r00);
13054 IkReal x608=((1.0)*x602);
13055 IkReal x609=(cj5*x602);
13056 IkReal x610=(cj4*x603);
13057 evalcond[0]=(((sj5*x603))+new_r20);
13058 evalcond[1]=((((-1.0)*sj5*x608))+new_r21);
13059 evalcond[2]=(((new_r11*sj4))+x605+x609);
13060 evalcond[3]=(((cj4*new_r10))+(((-1.0)*x608))+(((-1.0)*new_r00*x604)));
13061 evalcond[4]=(((cj4*new_r11))+(((-1.0)*x603))+(((-1.0)*new_r01*x604)));
13062 evalcond[5]=(((sj4*x603))+new_r01+((cj4*x609)));
13063 evalcond[6]=((((-1.0)*x603*x606))+((new_r10*sj4))+x607);
13064 evalcond[7]=(((sj4*x602))+(((-1.0)*x606*x610))+new_r00);
13065 evalcond[8]=(((sj4*x609))+(((-1.0)*x610))+new_r11);
13066 evalcond[9]=((((-1.0)*cj5*x603*x604))+(((-1.0)*cj4*x608))+new_r10);
13067 evalcond[10]=((((-1.0)*cj5*new_r10*x604))+((new_r20*sj5))+x603+(((-1.0)*x606*x607)));
13068 evalcond[11]=((((-1.0)*x605*x606))+(((-1.0)*x608))+((new_r21*sj5))+(((-1.0)*cj5*new_r11*x604)));
13076 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
13077 vinfos[0].jointtype = 17;
13078 vinfos[0].foffset = j0;
13079 vinfos[0].indices[0] = _ij0[0];
13080 vinfos[0].indices[1] = _ij0[1];
13081 vinfos[0].maxsolutions = _nj0;
13082 vinfos[1].jointtype = 1;
13083 vinfos[1].foffset = j1;
13084 vinfos[1].indices[0] = _ij1[0];
13085 vinfos[1].indices[1] = _ij1[1];
13086 vinfos[1].maxsolutions = _nj1;
13087 vinfos[2].jointtype = 1;
13088 vinfos[2].foffset = j2;
13089 vinfos[2].indices[0] = _ij2[0];
13090 vinfos[2].indices[1] = _ij2[1];
13091 vinfos[2].maxsolutions = _nj2;
13092 vinfos[3].jointtype = 1;
13093 vinfos[3].foffset = j3;
13094 vinfos[3].indices[0] = _ij3[0];
13095 vinfos[3].indices[1] = _ij3[1];
13096 vinfos[3].maxsolutions = _nj3;
13097 vinfos[4].jointtype = 1;
13098 vinfos[4].foffset = j4;
13099 vinfos[4].indices[0] = _ij4[0];
13100 vinfos[4].indices[1] = _ij4[1];
13101 vinfos[4].maxsolutions = _nj4;
13102 vinfos[5].jointtype = 1;
13103 vinfos[5].foffset = j5;
13104 vinfos[5].indices[0] = _ij5[0];
13105 vinfos[5].indices[1] = _ij5[1];
13106 vinfos[5].maxsolutions = _nj5;
13107 vinfos[6].jointtype = 1;
13108 vinfos[6].foffset = j6;
13109 vinfos[6].indices[0] = _ij6[0];
13110 vinfos[6].indices[1] = _ij6[1];
13111 vinfos[6].maxsolutions = _nj6;
13112 std::vector<int> vfree(0);
13130 }
static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3],
int& numroots)
13132 using std::complex;
13133 if( rawcoeffs[0] == 0 ) {
13135 polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
13139 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
13140 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
13141 complex<IkReal> coeffs[3];
13142 const int maxsteps = 110;
13143 for(
int i = 0; i < 3; ++i) {
13144 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
13146 complex<IkReal> roots[3];
13148 roots[0] = complex<IkReal>(1,0);
13149 roots[1] = complex<IkReal>(0.4,0.9);
13152 for(
int i = 2; i < 3; ++i) {
13153 roots[i] = roots[i-1]*roots[1];
13157 bool changed =
false;
13158 for(
int i = 0; i < 3; ++i) {
13159 if ( err[i] >= tol ) {
13162 complex<IkReal>
x = roots[i] + coeffs[0];
13163 for(
int j = 1; j < 3; ++j) {
13164 x = roots[i] * x + coeffs[j];
13166 for(
int j = 0; j < 3; ++j) {
13168 if( roots[i] != roots[j] ) {
13169 x /= (roots[i] - roots[j]);
13183 bool visited[3] = {
false};
13184 for(
int i = 0; i < 3; ++i) {
13185 if( !visited[i] ) {
13188 complex<IkReal> newroot=roots[i];
13190 for(
int j = i+1; j < 3; ++j) {
13192 if(
abs(real(roots[i])-real(roots[j])) < tolsqrt &&
abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
13193 newroot += roots[j];
13202 if(
IKabs(imag(newroot)) < tolsqrt ) {
13203 rawroots[numroots++] = real(newroot);
13208 static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2],
int& numroots) {
13209 IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
13213 else if( det == 0 ) {
13214 rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
13219 rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
13220 rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);
13224 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4],
int& numroots)
13226 using std::complex;
13227 if( rawcoeffs[0] == 0 ) {
13229 polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
13233 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
13234 const IkReal tolsqrt =
sqrt(std::numeric_limits<IkReal>::epsilon());
13235 complex<IkReal> coeffs[4];
13236 const int maxsteps = 110;
13237 for(
int i = 0; i < 4; ++i) {
13238 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
13240 complex<IkReal> roots[4];
13242 roots[0] = complex<IkReal>(1,0);
13243 roots[1] = complex<IkReal>(0.4,0.9);
13246 for(
int i = 2; i < 4; ++i) {
13247 roots[i] = roots[i-1]*roots[1];
13251 bool changed =
false;
13252 for(
int i = 0; i < 4; ++i) {
13253 if ( err[i] >= tol ) {
13256 complex<IkReal>
x = roots[i] + coeffs[0];
13257 for(
int j = 1; j < 4; ++j) {
13258 x = roots[i] * x + coeffs[j];
13260 for(
int j = 0; j < 4; ++j) {
13262 if( roots[i] != roots[j] ) {
13263 x /= (roots[i] - roots[j]);
13277 bool visited[4] = {
false};
13278 for(
int i = 0; i < 4; ++i) {
13279 if( !visited[i] ) {
13282 complex<IkReal> newroot=roots[i];
13284 for(
int j = i+1; j < 4; ++j) {
13286 if(
abs(real(roots[i])-real(roots[j])) < tolsqrt &&
abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
13287 newroot += roots[j];
13296 if(
IKabs(imag(newroot)) < tolsqrt ) {
13297 rawroots[numroots++] = real(newroot);
13309 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
13314 return solver.
ComputeIk(eetrans,eerot,pfree,solutions);
13321 #ifdef IKFAST_NAMESPACE 13325 #ifndef IKFAST_NO_MAIN 13327 #include <stdlib.h> 13328 #ifdef IKFAST_NAMESPACE 13329 using namespace IKFAST_NAMESPACE;
13334 printf(
"\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" 13335 "Returns the ik solutions given the transformation of the end effector specified by\n" 13336 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" 13343 IkReal eerot[9],eetrans[3];
13344 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
13345 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
13346 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
13347 for(std::size_t i = 0; i < vfree.size(); ++i)
13348 vfree[i] = atof(argv[13+i]);
13349 bool bSuccess =
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
13352 fprintf(stderr,
"Failed to get ik solution\n");
13356 printf(
"Found %d ik solutions:\n", (
int)solutions.
GetNumSolutions());
13360 printf(
"sol%d (free=%d): ", (
int)i, (
int)sol.
GetFree().size());
13361 std::vector<IkReal> vsolfree(sol.
GetFree().size());
13362 sol.
GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
13363 for( std::size_t j = 0; j < solvalues.size(); ++j)
13364 printf(
"%.15f, ", solvalues[j]);
int main(int argc, char **argv)
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
float IKatan2(float fy, float fx)
IKFAST_API const char * GetKinematicsHash()
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKFAST_API int GetIkRealSize()
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
IKFAST_API int GetNumJoints()
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
#define IKFAST_EVALCOND_THRESH
float IKfmod(float x, float y)
virtual size_t GetNumSolutions() const
returns the number of solutions stored
The discrete solutions are returned in this structure.
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
#define IKFAST_ATAN2_MAGTHRESH
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
IKFAST_API const char * GetIkFastVersion()
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
IKFAST_API int GetIkType()
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKFAST_API int GetNumFreeParameters()
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
IKFAST_API int * GetFreeParameters()
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
#define IKFAST_SOLUTION_THRESH
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
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)
Default implementation of IkSolutionListBase.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
float IKatan2Simple(float fy, float fx)
#define IKFAST_COMPILE_ASSERT(x)
#define IKFAST_SINCOS_THRESH
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
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)
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
manages all the solutions
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)