24#ifndef IKFAST_HAS_LIBRARY
25#define IKFAST_HAS_LIBRARY
32#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)(x)]
40#define IKFAST_STRINGIZE2(s) #s
41#define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
50#ifndef __PRETTY_FUNCTION__
51#define __PRETTY_FUNCTION__ __FUNCDNAME__
55#ifndef __PRETTY_FUNCTION__
56#define __PRETTY_FUNCTION__ __func__
59#define IKFAST_ASSERT(b) \
63 std::stringstream ss; \
64 ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " << __PRETTY_FUNCTION__ << ": Assertion '" \
65 << #b << "' failed"; \
66 throw std::runtime_error(ss.str()); \
73#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
75#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
78#define IK2PI ((IkReal)6.28318530717959)
79#define IKPI ((IkReal)3.14159265358979)
80#define IKPI_2 ((IkReal)1.57079632679490)
90void dgetrf_(
const int*
m,
const int* n,
double* a,
const int* lda,
int* ipiv,
int*
info);
91void zgetrf_(
const int*
m,
const int* n, std::complex<double>* a,
const int* lda,
int* ipiv,
int*
info);
92void dgetri_(
const int* n,
const double* a,
const int* lda,
int* ipiv,
double* work,
const int* lwork,
int*
info);
93void dgesv_(
const int* n,
const int* nrhs,
double* a,
const int* lda,
int* ipiv,
double* b,
const int* ldb,
int*
info);
121#ifdef IKFAST_NAMESPACE
122namespace IKFAST_NAMESPACE
126inline float IKabs(
float f) {
return fabsf(f); }
127inline double IKabs(
double f) {
return fabs(f); }
129inline float IKsqr(
float f) {
return f * f; }
130inline double IKsqr(
double f) {
return f * f; }
132inline float IKlog(
float f) {
return logf(f); }
133inline double IKlog(
double f) {
return log(f); }
136#ifndef IKFAST_SINCOS_THRESH
137#define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
141#ifndef IKFAST_ATAN2_MAGTHRESH
142#define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
146#ifndef IKFAST_SOLUTION_THRESH
147#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
211inline float IKsin(
float f) {
return sinf(f); }
212inline double IKsin(
double f) {
return sin(f); }
213inline float IKcos(
float f) {
return cosf(f); }
214inline double IKcos(
double f) {
return cos(f); }
215inline float IKtan(
float f) {
return tanf(f); }
216inline double IKtan(
double f) {
return tan(f); }
240 return atan2f(fy, fx);
253 return atan2(fy, fx);
284IKFAST_API
void ComputeFk(
const IkReal*
j, IkReal* eetrans, IkReal* eerot)
286 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,
287 x24, x25, x26, x27, x28, x29, x30, x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45, x46,
301 x12 = ((IkReal(0.0850000000000000)) * (x1));
302 x13 = ((IkReal(0.135000000000000)) * (x7));
303 x14 = ((IkReal(0.0850000000000000)) * (x3));
304 x15 = ((IkReal(1.00000000000000)) * (x7));
305 x16 = ((IkReal(1.00000000000000)) * (x2));
306 x17 = ((IkReal(0.0850000000000000)) * (x5));
307 x18 = ((IkReal(1.00000000000000)) * (x9));
308 x19 = ((IkReal(1.00000000000000)) * (x10));
309 x20 = ((IkReal(1.00000000000000)) * (x0));
310 x21 = ((IkReal(0.755000000000000)) * (x5));
311 x22 = ((IkReal(1.00000000000000)) * (x5));
319 x30 = ((x15) * (x6));
320 x31 = ((x20) * (x27));
321 x32 = ((x16) * (x27));
322 x33 = ((x27) + (((IkReal(-1.00000000000000)) * (x30))));
323 x34 = ((x30) + (((IkReal(-1.00000000000000)) * (x22) * (x4))));
324 x35 = ((((x22) * (x6))) + (((x15) * (x4))));
325 x36 = ((x1) * (x33));
326 x37 = ((x3) * (x34));
327 x38 = ((x10) * (x36));
328 x39 = ((((IkReal(-1.00000000000000)) * (x0) * (x30))) + (x31));
329 x40 = ((((IkReal(-1.00000000000000)) * (x2) * (x30))) + (x32));
330 x41 = ((((x15) * (x23))) + (((x20) * (x25))));
331 x42 = ((IkReal(-1.00000000000000)) * (x41));
332 x43 = ((((x15) * (x26))) + (((x16) * (x25))));
333 x44 = ((IkReal(-1.00000000000000)) * (x43));
334 x45 = ((((x1) * (x42))) + (((IkReal(-1.00000000000000)) * (x16) * (x3))));
335 x46 = ((((x3) * (x41))) + (((IkReal(-1.00000000000000)) * (x1) * (x16))));
336 x47 = ((((x3) * (x43))) + (((x0) * (x1))));
337 x48 = ((((x0) * (x3))) + (((x1) * (x44))));
338 x49 = ((x10) * (x45));
339 eerot[0] = ((((x46) * (x8))) + (((x11) * (((x49) + (((x39) * (x9))))))));
342 (((((IkReal(-1.00000000000000)) * (x18) * (x39))) + (((IkReal(-1.00000000000000)) * (x19) * (x45))))))) +
344 eerot[2] = ((((x45) * (x9))) + (((x10) * (((((IkReal(-1.00000000000000)) * (x31))) + (((x0) * (x24))))))));
345 IkReal x50 = ((x0) * (x24));
346 IkReal x51 = ((IkReal(1.00000000000000)) * (x23));
348 ((((IkReal(0.135000000000000)) * (x0) * (x25))) + (((IkReal(0.755000000000000)) * (x50))) +
349 (((IkReal(-1.00000000000000)) * (x21) * (x51))) +
350 (((x10) * (((((IkReal(0.0850000000000000)) * (x50))) + (((IkReal(-1.00000000000000)) * (x17) * (x51))))))) +
351 (((x13) * (x23))) + (((IkReal(0.100000000000000)) * (x0))) +
352 (((x9) * (((((x12) * (x42))) + (((IkReal(-1.00000000000000)) * (x14) * (x2))))))) +
353 (((IkReal(0.705000000000000)) * (x23))));
354 eerot[3] = ((((x47) * (x8))) + (((x11) * (((((x40) * (x9))) + (((x10) * (x48))))))));
355 eerot[4] = ((((x11) * (x47))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x40))) +
356 (((IkReal(-1.00000000000000)) * (x19) * (x48))))))));
357 eerot[5] = ((((x48) * (x9))) + (((x10) * (((((IkReal(-1.00000000000000)) * (x32))) + (((x2) * (x24))))))));
358 IkReal x52 = ((x2) * (x24));
359 IkReal x53 = ((IkReal(1.00000000000000)) * (x26));
361 ((((IkReal(0.755000000000000)) * (x52))) + (((x13) * (x26))) +
362 (((x10) * (((((IkReal(0.0850000000000000)) * (x52))) + (((IkReal(-1.00000000000000)) * (x17) * (x53))))))) +
363 (((x9) * (((((x0) * (x14))) + (((x12) * (x44))))))) + (((IkReal(0.100000000000000)) * (x2))) +
364 (((IkReal(-1.00000000000000)) * (x21) * (x53))) + (((IkReal(0.705000000000000)) * (x26))) +
365 (((IkReal(0.135000000000000)) * (x2) * (x25))));
366 eerot[6] = ((((x11) * (((((x35) * (x9))) + (x38))))) + (((x37) * (x8))));
367 eerot[7] = ((((x11) * (x37))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x35))) +
368 (((IkReal(-1.00000000000000)) * (x19) * (x36))))))));
369 eerot[8] = ((((IkReal(-1.00000000000000)) * (x10) * (x35))) + (((x28) * (x33))));
370 IkReal x54 = ((IkReal(1.00000000000000)) * (x6));
372 ((IkReal(0.615000000000000)) +
373 (((x10) * (((((IkReal(-0.0850000000000000)) * (x29))) + (((IkReal(-1.00000000000000)) * (x17) * (x54))))))) +
374 (((x13) * (x6))) + (((x28) * (((((IkReal(-0.0850000000000000)) * (x24))) + (((x17) * (x4))))))) +
375 (((IkReal(-1.00000000000000)) * (x21) * (x54))) + (((IkReal(0.705000000000000)) * (x6))) +
376 (((IkReal(-0.755000000000000)) * (x29))) + (((IkReal(-0.135000000000000)) * (x27))));
390 IkReal j0,
cj0, sj0, htj0, j1, cj1, sj1, htj1, j2, cj2, sj2, htj2, j3, cj3, sj3, htj3, j4, cj4, sj4, htj4, j5, cj5,
391 sj5,
htj5, new_r00, r00, rxp0_0, new_r01, r01, rxp0_1, new_r02, r02, rxp0_2, new_r10, r10, rxp1_0, new_r11, r11,
392 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,
394 unsigned char _ij0[2], _nj0, _ij1[2], _nj1, _ij2[2], _nj2, _ij3[2], _nj3, _ij4[2], _nj4, _ij5[2], _nj5;
398 j0 = numeric_limits<IkReal>::quiet_NaN();
402 j1 = numeric_limits<IkReal>::quiet_NaN();
406 j2 = numeric_limits<IkReal>::quiet_NaN();
410 j3 = numeric_limits<IkReal>::quiet_NaN();
414 j4 = numeric_limits<IkReal>::quiet_NaN();
418 j5 = numeric_limits<IkReal>::quiet_NaN();
422 for (
int dummyiter = 0; dummyiter < 1; ++dummyiter)
425 r00 = eerot[0 * 3 + 0];
426 r01 = eerot[0 * 3 + 1];
427 r02 = eerot[0 * 3 + 2];
428 r10 = eerot[1 * 3 + 0];
429 r11 = eerot[1 * 3 + 1];
430 r12 = eerot[1 * 3 + 2];
431 r20 = eerot[2 * 3 + 0];
432 r21 = eerot[2 * 3 + 1];
433 r22 = eerot[2 * 3 + 2];
441 new_px = ((((IkReal(-0.0850000000000000)) * (r02))) + (px));
445 new_py = ((((IkReal(-0.0850000000000000)) * (r12))) + (py));
449 new_pz = ((IkReal(-0.615000000000000)) + (((IkReal(-0.0850000000000000)) * (r22))) + (pz));
462 pp = (((px) * (px)) + ((pz) * (pz)) + ((py) * (py)));
463 npx = ((((py) * (r10))) + (((pz) * (r20))) + (((px) * (r00))));
464 npy = ((((px) * (r01))) + (((pz) * (r21))) + (((py) * (r11))));
465 npz = ((((py) * (r12))) + (((pz) * (r22))) + (((px) * (r02))));
466 rxp0_0 = ((((IkReal(-1.00000000000000)) * (py) * (r20))) + (((pz) * (r10))));
467 rxp0_1 = ((((px) * (r20))) + (((IkReal(-1.00000000000000)) * (pz) * (r00))));
468 rxp0_2 = ((((py) * (r00))) + (((IkReal(-1.00000000000000)) * (px) * (r10))));
469 rxp1_0 = ((((pz) * (r11))) + (((IkReal(-1.00000000000000)) * (py) * (r21))));
470 rxp1_1 = ((((IkReal(-1.00000000000000)) * (pz) * (r01))) + (((px) * (r21))));
471 rxp1_2 = ((((py) * (r01))) + (((IkReal(-1.00000000000000)) * (px) * (r11))));
472 rxp2_0 = ((((IkReal(-1.00000000000000)) * (py) * (r22))) + (((pz) * (r12))));
473 rxp2_1 = ((((px) * (r22))) + (((IkReal(-1.00000000000000)) * (pz) * (r02))));
474 rxp2_2 = ((((py) * (r02))) + (((IkReal(-1.00000000000000)) * (px) * (r12))));
476 IkReal j0array[2], cj0array[2], sj0array[2];
477 bool j0valid[2] = {
false };
481 IkReal x55 =
IKatan2(((IkReal(-1.00000000000000)) * (py)), px);
482 j0array[0] = ((IkReal(-1.00000000000000)) * (x55));
483 sj0array[0] =
IKsin(j0array[0]);
484 cj0array[0] =
IKcos(j0array[0]);
485 j0array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x55))));
486 sj0array[1] =
IKsin(j0array[1]);
487 cj0array[1] =
IKcos(j0array[1]);
488 if (j0array[0] >
IKPI)
492 else if (j0array[0] < -
IKPI)
497 if (j0array[1] >
IKPI)
501 else if (j0array[1] < -
IKPI)
506 for (
int ij0 = 0; ij0 < 2; ++ij0)
514 for (
int iij0 = ij0 + 1; iij0 < 2; ++iij0)
519 j0valid[iij0] =
false;
529 IkReal j2array[2], cj2array[2], sj2array[2];
530 bool j2valid[2] = {
false };
532 if ((((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
533 (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp))))) <
535 (((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
536 (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp))))) >
540 IKasin(((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
541 (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp)))));
542 j2array[0] = ((IkReal(-2.96465459743209)) + (((IkReal(-1.00000000000000)) * (x56))));
543 sj2array[0] =
IKsin(j2array[0]);
544 cj2array[0] =
IKcos(j2array[0]);
545 j2array[1] = ((IkReal(0.176938056157703)) + (x56));
546 sj2array[1] =
IKsin(j2array[1]);
547 cj2array[1] =
IKcos(j2array[1]);
548 if (j2array[0] >
IKPI)
552 else if (j2array[0] < -
IKPI)
557 if (j2array[1] >
IKPI)
561 else if (j2array[1] < -
IKPI)
566 for (
int ij2 = 0; ij2 < 2; ++ij2)
574 for (
int iij2 = ij2 + 1; iij2 < 2; ++iij2)
579 j2valid[iij2] =
false;
591 IkReal x57 = ((IkReal(0.755000000000000)) * (cj2));
592 IkReal x58 = ((py) * (sj0));
593 IkReal x59 = ((cj0) * (px));
594 IkReal x60 = ((IkReal(0.135000000000000)) * (sj2));
596 ((((IkReal(0.705000000000000)) * (pz))) + (((IkReal(-1.00000000000000)) * (x58) * (x60))) +
597 (((IkReal(0.0755000000000000)) * (cj2))) + (((IkReal(-0.755000000000000)) * (pz) * (sj2))) +
598 (((IkReal(-1.00000000000000)) * (x57) * (x58))) + (((IkReal(-1.00000000000000)) * (x59) * (x60))) +
599 (((IkReal(-1.00000000000000)) * (x57) * (x59))) + (((IkReal(0.0135000000000000)) * (sj2))) +
600 (((IkReal(0.135000000000000)) * (cj2) * (pz)))));
601 IkReal x61 = ((IkReal(10.0000000000000)) * (sj2));
602 IkReal x62 = ((cj0) * (px));
603 IkReal x63 = ((py) * (sj0));
604 IkReal x64 = ((IkReal(55.9259259259259)) * (cj2));
606 ((((IkReal(52.2222222222222)) * (pz))) + (((IkReal(-1.00000000000000)) * (x62) * (x64))) + (sj2) +
607 (((IkReal(10.0000000000000)) * (cj2) * (pz))) + (((IkReal(-1.00000000000000)) * (x63) * (x64))) +
608 (((IkReal(-1.00000000000000)) * (x61) * (x62))) + (((IkReal(-1.00000000000000)) * (x61) * (x63))) +
609 (((IkReal(5.59259259259259)) * (cj2))) + (((IkReal(-55.9259259259259)) * (pz) * (sj2))));
610 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
615 IkReal x65 = ((IkReal(0.755000000000000)) * (sj2));
616 IkReal x66 = ((cj0) * (px));
617 IkReal x67 = ((py) * (sj0));
618 IkReal x68 = ((IkReal(0.135000000000000)) * (cj2));
620 ((IkReal(0.0705000000000000)) + (((x65) * (x66))) + (((IkReal(-0.705000000000000)) * (x67))) +
621 (((x65) * (x67))) + (((IkReal(-0.755000000000000)) * (cj2) * (pz))) +
622 (((IkReal(-0.705000000000000)) * (x66))) + (((IkReal(-1.00000000000000)) * (x66) * (x68))) +
623 (((IkReal(-1.00000000000000)) * (x67) * (x68))) + (((IkReal(0.0135000000000000)) * (cj2))) +
624 (((IkReal(-0.135000000000000)) * (pz) * (sj2))) + (((IkReal(-0.0755000000000000)) * (sj2)))));
625 IkReal x69 = ((cj0) * (px));
626 IkReal x70 = ((IkReal(10.0000000000000)) * (cj2));
627 IkReal x71 = ((IkReal(55.9259259259259)) * (sj2));
628 IkReal x72 = ((py) * (sj0));
630 ((IkReal(5.22222222222222)) + (((IkReal(-1.00000000000000)) * (x70) * (x72))) +
631 (((x71) * (x72))) + (((IkReal(-52.2222222222222)) * (x72))) +
632 (((IkReal(-10.0000000000000)) * (pz) * (sj2))) + (cj2) +
633 (((IkReal(-52.2222222222222)) * (x69))) + (((IkReal(-1.00000000000000)) * (x69) * (x70))) +
634 (((IkReal(-5.59259259259259)) * (sj2))) + (((IkReal(-55.9259259259259)) * (cj2) * (pz))) +
636 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
643 IkReal j1array[1], cj1array[1], sj1array[1];
644 bool j1valid[1] = {
false };
646 IkReal x73 = (sj2) * (sj2);
647 IkReal x74 = (cj2) * (cj2);
648 IkReal x75 = ((cj2) * (sj2));
649 IkReal x76 = ((IkReal(1.00000000000000)) * (pz));
652 (((IkReal(-0.497025000000000)) + (((IkReal(-0.190350000000000)) * (cj2))) +
653 ((pz) * (pz)) + (((IkReal(1.06455000000000)) * (sj2))) +
654 (((IkReal(0.203850000000000)) * (x75))) + (((IkReal(-0.570025000000000)) * (x73))) +
658 (((((IkReal(0.101925000000000)) * (x73))) + (((IkReal(-0.101925000000000)) * (x74))) +
659 (((IkReal(0.100000000000000)) * (pz))) + (((IkReal(0.551800000000000)) * (x75))) +
660 (((IkReal(-1.00000000000000)) * (py) * (sj0) * (x76))) +
661 (((IkReal(-0.532275000000000)) * (cj2))) + (((IkReal(-0.0951750000000000)) * (sj2))) +
666 (((IkReal(-0.497025000000000)) + (((IkReal(-0.190350000000000)) * (cj2))) + ((pz) * (pz)) +
667 (((IkReal(1.06455000000000)) * (sj2))) + (((IkReal(0.203850000000000)) * (x75))) +
668 (((IkReal(-0.570025000000000)) * (x73))) + (((IkReal(-0.0182250000000000)) * (x74)))))),
670 (((((IkReal(0.101925000000000)) * (x73))) + (((IkReal(-0.101925000000000)) * (x74))) +
671 (((IkReal(0.100000000000000)) * (pz))) + (((IkReal(0.551800000000000)) * (x75))) +
672 (((IkReal(-1.00000000000000)) * (py) * (sj0) * (x76))) +
673 (((IkReal(-0.532275000000000)) * (cj2))) + (((IkReal(-0.0951750000000000)) * (sj2))) +
674 (((IkReal(-1.00000000000000)) * (cj0) * (px) * (x76)))))));
675 sj1array[0] =
IKsin(j1array[0]);
676 cj1array[0] =
IKcos(j1array[0]);
677 if (j1array[0] >
IKPI)
681 else if (j1array[0] < -
IKPI)
686 for (
int ij1 = 0; ij1 < 1; ++ij1)
694 for (
int iij1 = ij1 + 1; iij1 < 1; ++iij1)
699 j1valid[iij1] =
false;
709 IkReal x77 =
IKsin(j1);
710 IkReal x78 =
IKcos(j1);
711 IkReal x79 = ((IkReal(0.135000000000000)) * (sj2));
712 IkReal x80 = ((cj0) * (px));
713 IkReal x81 = ((IkReal(0.755000000000000)) * (sj2));
714 IkReal x82 = ((py) * (sj0));
715 IkReal x83 = ((IkReal(0.755000000000000)) * (cj2));
716 IkReal x84 = ((IkReal(0.135000000000000)) * (cj2));
717 IkReal x85 = ((IkReal(0.135000000000000)) * (x78));
718 IkReal x86 = ((IkReal(1.00000000000000)) * (x78));
719 IkReal x87 = ((IkReal(1.41000000000000)) * (x77));
720 IkReal x88 = ((pz) * (x78));
721 evalcond[0] = ((IkReal(-0.705000000000000)) + (((x77) * (x80))) + (((x77) * (x82))) +
722 (((IkReal(-0.100000000000000)) * (x77))) +
723 (((IkReal(-1.00000000000000)) * (x84))) + (x88) + (x81));
724 evalcond[1] = ((((IkReal(0.100000000000000)) * (x78))) +
725 (((IkReal(-1.00000000000000)) * (x80) * (x86))) + (x79) +
726 (((IkReal(-1.00000000000000)) * (x82) * (x86))) + (x83) + (((pz) * (x77))));
727 evalcond[2] = ((((IkReal(-0.705000000000000)) * (x78))) +
728 (((IkReal(-1.00000000000000)) * (x78) * (x84))) + (((x77) * (x79))) + (pz) +
729 (((x78) * (x81))) + (((x77) * (x83))));
731 ((IkReal(0.0812250000000000)) + (((IkReal(-0.141000000000000)) * (x77))) +
732 (((IkReal(-1.00000000000000)) * (pp))) + (((x82) * (x87))) +
733 (((IkReal(0.200000000000000)) * (x82))) + (((IkReal(1.41000000000000)) * (x88))) +
734 (((x80) * (x87))) + (((IkReal(0.200000000000000)) * (x80))));
736 ((IkReal(0.100000000000000)) + (((IkReal(-1.00000000000000)) * (x82))) +
737 (((x77) * (x84))) + (((IkReal(-1.00000000000000)) * (x77) * (x81))) +
738 (((x78) * (x83))) + (((x78) * (x79))) + (((IkReal(0.705000000000000)) * (x77))) +
739 (((IkReal(-1.00000000000000)) * (x80))));
740 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
741 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
742 IKabs(evalcond[4]) > 0.000001)
748 rotationfunction0(solutions);
757 IkReal j1array[1], cj1array[1], sj1array[1];
758 bool j1valid[1] = {
false };
760 IkReal x228 = (sj2) * (sj2);
761 IkReal x229 = (cj2) * (cj2);
762 IkReal x230 = ((cj2) * (sj2));
763 if (
IKabs(((gconst1) *
764 (((((IkReal(0.551800000000000)) * (x230))) + (((IkReal(0.101925000000000)) * (x228))) +
765 (((IkReal(-0.100000000000000)) * (pz))) + (((IkReal(-0.101925000000000)) * (x229))) +
766 (((cj0) * (px) * (pz))) + (((IkReal(-0.532275000000000)) * (cj2))) +
767 (((IkReal(-0.0951750000000000)) * (sj2))) + (((py) * (pz) * (sj0))))))) <
769 IKabs(((gconst1) * (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) +
770 (((IkReal(-0.203850000000000)) * (x230))) +
775 (((((IkReal(0.551800000000000)) * (x230))) + (((IkReal(0.101925000000000)) * (x228))) +
776 (((IkReal(-0.100000000000000)) * (pz))) + (((IkReal(-0.101925000000000)) * (x229))) +
777 (((cj0) * (px) * (pz))) + (((IkReal(-0.532275000000000)) * (cj2))) +
778 (((IkReal(-0.0951750000000000)) * (sj2))) + (((py) * (pz) * (sj0)))))),
779 ((gconst1) * (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) +
780 (((IkReal(-0.203850000000000)) * (x230))) +
781 (((IkReal(-0.570025000000000)) * (x229)))))));
782 sj1array[0] =
IKsin(j1array[0]);
783 cj1array[0] =
IKcos(j1array[0]);
784 if (j1array[0] >
IKPI)
788 else if (j1array[0] < -
IKPI)
793 for (
int ij1 = 0; ij1 < 1; ++ij1)
801 for (
int iij1 = ij1 + 1; iij1 < 1; ++iij1)
806 j1valid[iij1] =
false;
816 IkReal x231 =
IKsin(j1);
817 IkReal x232 =
IKcos(j1);
818 IkReal x233 = ((IkReal(0.135000000000000)) * (sj2));
819 IkReal x234 = ((cj0) * (px));
820 IkReal x235 = ((IkReal(0.755000000000000)) * (sj2));
821 IkReal x236 = ((py) * (sj0));
822 IkReal x237 = ((IkReal(0.755000000000000)) * (cj2));
823 IkReal x238 = ((IkReal(0.135000000000000)) * (cj2));
824 IkReal x239 = ((IkReal(0.135000000000000)) * (x232));
825 IkReal x240 = ((IkReal(1.00000000000000)) * (x232));
826 IkReal x241 = ((IkReal(1.41000000000000)) * (x231));
827 IkReal x242 = ((pz) * (x232));
828 evalcond[0] = ((IkReal(-0.705000000000000)) + (((IkReal(-1.00000000000000)) * (x238))) +
829 (x235) + (((x231) * (x236))) + (x242) +
830 (((IkReal(-0.100000000000000)) * (x231))) + (((x231) * (x234))));
831 evalcond[1] = ((((IkReal(0.100000000000000)) * (x232))) + (((pz) * (x231))) + (x233) + (x237) +
832 (((IkReal(-1.00000000000000)) * (x234) * (x240))) +
833 (((IkReal(-1.00000000000000)) * (x236) * (x240))));
835 ((((IkReal(-0.705000000000000)) * (x232))) + (((x231) * (x237))) + (((x231) * (x233))) +
836 (pz) + (((x232) * (x235))) + (((IkReal(-1.00000000000000)) * (x232) * (x238))));
838 ((IkReal(0.0812250000000000)) + (((x236) * (x241))) +
839 (((IkReal(0.200000000000000)) * (x236))) + (((IkReal(0.200000000000000)) * (x234))) +
840 (((IkReal(-1.00000000000000)) * (pp))) + (((x234) * (x241))) +
841 (((IkReal(1.41000000000000)) * (x242))) + (((IkReal(-0.141000000000000)) * (x231))));
843 ((IkReal(0.100000000000000)) + (((IkReal(-1.00000000000000)) * (x231) * (x235))) +
844 (((IkReal(0.705000000000000)) * (x231))) + (((IkReal(-1.00000000000000)) * (x236))) +
845 (((x231) * (x238))) + (((x232) * (x233))) + (((x232) * (x237))) +
846 (((IkReal(-1.00000000000000)) * (x234))));
847 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
848 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
849 IKabs(evalcond[4]) > 0.000001)
855 rotationfunction0(solutions);
869 for (
int rotationiter = 0; rotationiter < 1; ++rotationiter)
871 IkReal x89 = ((cj0) * (r00));
872 IkReal x90 = ((cj0) * (r01));
873 IkReal x91 = ((sj1) * (sj2));
874 IkReal x92 = ((IkReal(1.00000000000000)) * (sj0));
875 IkReal x93 = ((r10) * (sj0));
876 IkReal x94 = ((IkReal(1.00000000000000)) * (cj2));
877 IkReal x95 = ((r12) * (sj0));
878 IkReal x96 = ((cj0) * (r02));
879 IkReal x97 = ((r11) * (sj0));
880 IkReal x98 = ((((IkReal(-1.00000000000000)) * (cj1) * (x94))) + (x91));
881 IkReal x99 = ((((IkReal(-1.00000000000000)) * (x91))) + (((cj1) * (cj2))));
882 IkReal x100 = ((cj0) * (x99));
883 IkReal x101 = ((((IkReal(-1.00000000000000)) * (sj1) * (x94))) + (((IkReal(-1.00000000000000)) * (cj1) * (sj2))));
884 IkReal x102 = ((sj0) * (x101));
885 new_r00 = ((((r20) * (x98))) + (((x101) * (x89))) + (((x101) * (x93))));
886 new_r01 = ((((x101) * (x90))) + (((x101) * (x97))) + (((r21) * (x98))));
887 new_r02 = ((((x101) * (x95))) + (((r22) * (x98))) + (((x101) * (x96))));
888 new_r10 = ((((IkReal(-1.00000000000000)) * (r00) * (x92))) + (((cj0) * (r10))));
889 new_r11 = ((((IkReal(-1.00000000000000)) * (r01) * (x92))) + (((cj0) * (r11))));
890 new_r12 = ((((IkReal(-1.00000000000000)) * (r02) * (x92))) + (((cj0) * (r12))));
891 new_r20 = ((((x93) * (x99))) + (((x89) * (x99))) + (((r20) * (x101))));
892 new_r21 = ((((r21) * (x101))) + (((x97) * (x99))) + (((x90) * (x99))));
893 new_r22 = ((((x96) * (x99))) + (((r22) * (x101))) + (((x95) * (x99))));
895 IkReal j4array[2], cj4array[2], sj4array[2];
896 bool j4valid[2] = {
false };
898 cj4array[0] = new_r22;
901 j4valid[0] = j4valid[1] =
true;
902 j4array[0] =
IKacos(cj4array[0]);
903 sj4array[0] =
IKsin(j4array[0]);
904 cj4array[1] = cj4array[0];
905 j4array[1] = -j4array[0];
906 sj4array[1] = -sj4array[0];
908 else if (isnan(cj4array[0]))
916 for (
int ij4 = 0; ij4 < 2; ++ij4)
924 for (
int iij4 = ij4 + 1; iij4 < 2; ++iij4)
929 j4valid[iij4] =
false;
943 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
948 gconst2 =
IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02))));
949 dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02)));
950 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
955 gconst3 =
IKsign(((((new_r10) * (new_r12) * (sj4))) + (((new_r00) * (new_r02) * (sj4)))));
956 dummyeval[0] = ((((new_r10) * (new_r12) * (sj4))) + (((new_r00) * (new_r02) * (sj4))));
957 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
961 IkReal x103 = ((IkReal(-1.00000000000000)) + (new_r22));
962 evalcond[0] = ((IkReal(-3.14159265358979)) +
963 (
IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
965 evalcond[2] = new_r20;
966 evalcond[3] = new_r21;
967 evalcond[4] = new_r20;
968 evalcond[5] = new_r21;
970 if (
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
971 IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
972 IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
973 IKabs(evalcond[6]) < 0.0000010000000000)
976 IkReal j3array[2], cj3array[2], sj3array[2];
977 bool j3valid[2] = {
false };
981 IkReal x104 =
IKatan2(new_r02, new_r12);
982 j3array[0] = ((IkReal(-1.00000000000000)) * (x104));
983 sj3array[0] =
IKsin(j3array[0]);
984 cj3array[0] =
IKcos(j3array[0]);
985 j3array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x104))));
986 sj3array[1] =
IKsin(j3array[1]);
987 cj3array[1] =
IKcos(j3array[1]);
988 if (j3array[0] >
IKPI)
992 else if (j3array[0] < -
IKPI)
997 if (j3array[1] >
IKPI)
1001 else if (j3array[1] < -
IKPI)
1003 j3array[1] +=
IK2PI;
1006 for (
int ij3 = 0; ij3 < 2; ++ij3)
1014 for (
int iij3 = ij3 + 1; iij3 < 2; ++iij3)
1019 j3valid[iij3] =
false;
1025 cj3 = cj3array[ij3];
1026 sj3 = sj3array[ij3];
1029 evalcond[0] = ((((new_r12) * (
IKcos(j3)))) +
1030 (((IkReal(-1.00000000000000)) * (new_r02) * (
IKsin(j3)))));
1031 if (
IKabs(evalcond[0]) > 0.000001)
1038 IkReal j5array[1], cj5array[1], sj5array[1];
1039 bool j5valid[1] = {
false };
1041 if (
IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1042 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
1045 IKabs(
IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1046 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) +
1047 IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - 1) <=
1050 j5array[0] =
IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1051 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))),
1052 ((((new_r10) * (sj3))) + (((cj3) * (new_r00)))));
1053 sj5array[0] =
IKsin(j5array[0]);
1054 cj5array[0] =
IKcos(j5array[0]);
1055 if (j5array[0] >
IKPI)
1057 j5array[0] -=
IK2PI;
1059 else if (j5array[0] < -
IKPI)
1061 j5array[0] +=
IK2PI;
1064 for (
int ij5 = 0; ij5 < 1; ++ij5)
1072 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1074 if (j5valid[iij5] &&
1078 j5valid[iij5] =
false;
1084 cj5 = cj5array[ij5];
1085 sj5 = sj5array[ij5];
1088 IkReal x105 =
IKsin(j5);
1089 IkReal x106 = ((IkReal(1.00000000000000)) * (sj3));
1090 IkReal x107 = ((IkReal(1.00000000000000)) * (
IKcos(j5)));
1091 evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x106))) +
1092 (((cj3) * (new_r10))) + (((IkReal(-1.00000000000000)) * (x105))));
1093 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x107))) + (((cj3) * (new_r11))) +
1094 (((IkReal(-1.00000000000000)) * (new_r01) * (x106))));
1095 evalcond[2] = ((((new_r11) * (sj3))) + (x105) + (((cj3) * (new_r01))));
1096 evalcond[3] = ((((IkReal(-1.00000000000000)) * (x107))) + (((new_r10) * (sj3))) +
1097 (((cj3) * (new_r00))));
1098 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1099 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001)
1106 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1107 vinfos[0].jointtype = 1;
1108 vinfos[0].foffset = j0;
1109 vinfos[0].indices[0] = _ij0[0];
1110 vinfos[0].indices[1] = _ij0[1];
1111 vinfos[0].maxsolutions = _nj0;
1112 vinfos[1].jointtype = 1;
1113 vinfos[1].foffset = j1;
1114 vinfos[1].indices[0] = _ij1[0];
1115 vinfos[1].indices[1] = _ij1[1];
1116 vinfos[1].maxsolutions = _nj1;
1117 vinfos[2].jointtype = 1;
1118 vinfos[2].foffset = j2;
1119 vinfos[2].indices[0] = _ij2[0];
1120 vinfos[2].indices[1] = _ij2[1];
1121 vinfos[2].maxsolutions = _nj2;
1122 vinfos[3].jointtype = 1;
1123 vinfos[3].foffset = j3;
1124 vinfos[3].indices[0] = _ij3[0];
1125 vinfos[3].indices[1] = _ij3[1];
1126 vinfos[3].maxsolutions = _nj3;
1127 vinfos[4].jointtype = 1;
1128 vinfos[4].foffset = j4;
1129 vinfos[4].indices[0] = _ij4[0];
1130 vinfos[4].indices[1] = _ij4[1];
1131 vinfos[4].maxsolutions = _nj4;
1132 vinfos[5].jointtype = 1;
1133 vinfos[5].foffset = j5;
1134 vinfos[5].indices[0] = _ij5[0];
1135 vinfos[5].indices[1] = _ij5[1];
1136 vinfos[5].maxsolutions = _nj5;
1137 std::vector<int> vfree(0);
1147 evalcond[0] = ((IkReal(-3.14159265358979)) + (
IKfmod(j4, IkReal(6.28318530717959))));
1148 evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22));
1149 evalcond[2] = new_r20;
1150 evalcond[3] = new_r21;
1151 evalcond[4] = ((IkReal(-1.00000000000000)) * (new_r20));
1152 evalcond[5] = ((IkReal(-1.00000000000000)) * (new_r21));
1153 evalcond[6] = ((IkReal(-1.00000000000000)) + (((IkReal(-1.00000000000000)) * (new_r22))));
1154 if (
IKabs(evalcond[0]) < 0.0000010000000000 &&
IKabs(evalcond[1]) < 0.0000010000000000 &&
1155 IKabs(evalcond[2]) < 0.0000010000000000 &&
IKabs(evalcond[3]) < 0.0000010000000000 &&
1156 IKabs(evalcond[4]) < 0.0000010000000000 &&
IKabs(evalcond[5]) < 0.0000010000000000 &&
1157 IKabs(evalcond[6]) < 0.0000010000000000)
1160 IkReal j3array[2], cj3array[2], sj3array[2];
1161 bool j3valid[2] = {
false };
1165 IkReal x108 =
IKatan2(new_r02, new_r12);
1166 j3array[0] = ((IkReal(-1.00000000000000)) * (x108));
1167 sj3array[0] =
IKsin(j3array[0]);
1168 cj3array[0] =
IKcos(j3array[0]);
1169 j3array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x108))));
1170 sj3array[1] =
IKsin(j3array[1]);
1171 cj3array[1] =
IKcos(j3array[1]);
1172 if (j3array[0] >
IKPI)
1174 j3array[0] -=
IK2PI;
1176 else if (j3array[0] < -
IKPI)
1178 j3array[0] +=
IK2PI;
1181 if (j3array[1] >
IKPI)
1183 j3array[1] -=
IK2PI;
1185 else if (j3array[1] < -
IKPI)
1187 j3array[1] +=
IK2PI;
1190 for (
int ij3 = 0; ij3 < 2; ++ij3)
1198 for (
int iij3 = ij3 + 1; iij3 < 2; ++iij3)
1203 j3valid[iij3] =
false;
1209 cj3 = cj3array[ij3];
1210 sj3 = sj3array[ij3];
1213 evalcond[0] = ((((new_r12) * (
IKcos(j3)))) +
1214 (((IkReal(-1.00000000000000)) * (new_r02) * (
IKsin(j3)))));
1215 if (
IKabs(evalcond[0]) > 0.000001)
1222 IkReal j5array[1], cj5array[1], sj5array[1];
1223 bool j5valid[1] = {
false };
1226 IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1227 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
1229 IKabs(
IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
1230 IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1231 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
1234 j5array[0] =
IKatan2(((((new_r11) * (sj3))) + (((cj3) * (new_r01)))),
1235 ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1236 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3)))));
1237 sj5array[0] =
IKsin(j5array[0]);
1238 cj5array[0] =
IKcos(j5array[0]);
1239 if (j5array[0] >
IKPI)
1241 j5array[0] -=
IK2PI;
1243 else if (j5array[0] < -
IKPI)
1245 j5array[0] +=
IK2PI;
1248 for (
int ij5 = 0; ij5 < 1; ++ij5)
1256 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1258 if (j5valid[iij5] &&
1262 j5valid[iij5] =
false;
1268 cj5 = cj5array[ij5];
1269 sj5 = sj5array[ij5];
1272 IkReal x109 =
IKcos(j5);
1273 IkReal x110 = ((IkReal(1.00000000000000)) * (sj3));
1274 IkReal x111 = ((IkReal(1.00000000000000)) * (
IKsin(j5)));
1275 evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x110))) +
1276 (((IkReal(-1.00000000000000)) * (x111))) + (((cj3) * (new_r10))));
1277 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x109))) +
1278 (((IkReal(-1.00000000000000)) * (new_r01) * (x110))) +
1279 (((cj3) * (new_r11))));
1280 evalcond[2] = ((((new_r11) * (sj3))) + (((IkReal(-1.00000000000000)) * (x111))) +
1281 (((cj3) * (new_r01))));
1282 evalcond[3] = ((((new_r10) * (sj3))) + (x109) + (((cj3) * (new_r00))));
1283 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1284 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001)
1291 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1292 vinfos[0].jointtype = 1;
1293 vinfos[0].foffset = j0;
1294 vinfos[0].indices[0] = _ij0[0];
1295 vinfos[0].indices[1] = _ij0[1];
1296 vinfos[0].maxsolutions = _nj0;
1297 vinfos[1].jointtype = 1;
1298 vinfos[1].foffset = j1;
1299 vinfos[1].indices[0] = _ij1[0];
1300 vinfos[1].indices[1] = _ij1[1];
1301 vinfos[1].maxsolutions = _nj1;
1302 vinfos[2].jointtype = 1;
1303 vinfos[2].foffset = j2;
1304 vinfos[2].indices[0] = _ij2[0];
1305 vinfos[2].indices[1] = _ij2[1];
1306 vinfos[2].maxsolutions = _nj2;
1307 vinfos[3].jointtype = 1;
1308 vinfos[3].foffset = j3;
1309 vinfos[3].indices[0] = _ij3[0];
1310 vinfos[3].indices[1] = _ij3[1];
1311 vinfos[3].maxsolutions = _nj3;
1312 vinfos[4].jointtype = 1;
1313 vinfos[4].foffset = j4;
1314 vinfos[4].indices[0] = _ij4[0];
1315 vinfos[4].indices[1] = _ij4[1];
1316 vinfos[4].maxsolutions = _nj4;
1317 vinfos[5].jointtype = 1;
1318 vinfos[5].foffset = j5;
1319 vinfos[5].indices[0] = _ij5[0];
1320 vinfos[5].indices[1] = _ij5[1];
1321 vinfos[5].maxsolutions = _nj5;
1322 std::vector<int> vfree(0);
1346 IkReal j3array[1], cj3array[1], sj3array[1];
1347 bool j3valid[1] = {
false };
1349 IkReal x112 = ((IkReal(-1.00000000000000)) * (cj4) * (gconst3) * (new_r20));
1353 j3array[0] =
IKatan2(((new_r12) * (x112)), ((new_r02) * (x112)));
1354 sj3array[0] =
IKsin(j3array[0]);
1355 cj3array[0] =
IKcos(j3array[0]);
1356 if (j3array[0] >
IKPI)
1358 j3array[0] -=
IK2PI;
1360 else if (j3array[0] < -
IKPI)
1362 j3array[0] +=
IK2PI;
1365 for (
int ij3 = 0; ij3 < 1; ++ij3)
1373 for (
int iij3 = ij3 + 1; iij3 < 1; ++iij3)
1378 j3valid[iij3] =
false;
1384 cj3 = cj3array[ij3];
1385 sj3 = sj3array[ij3];
1388 IkReal x113 =
IKsin(j3);
1389 IkReal x114 =
IKcos(j3);
1390 IkReal x115 = ((IkReal(1.00000000000000)) * (sj4));
1391 IkReal x116 = ((sj4) * (x113));
1392 IkReal x117 = ((sj4) * (x114));
1393 IkReal x118 = ((new_r02) * (x114));
1394 IkReal x119 = ((new_r12) * (x113));
1396 ((((IkReal(-1.00000000000000)) * (new_r02) * (x113))) + (((new_r12) * (x114))));
1397 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x115))) + (x119) + (x118));
1398 evalcond[2] = ((((new_r00) * (x117))) + (((cj4) * (new_r20))) + (((new_r10) * (x116))));
1399 evalcond[3] = ((((new_r11) * (x116))) + (((new_r01) * (x117))) + (((cj4) * (new_r21))));
1400 evalcond[4] = ((IkReal(-1.00000000000000)) + (((new_r02) * (x117))) +
1401 (((new_r12) * (x116))) + (((cj4) * (new_r22))));
1402 evalcond[5] = ((((cj4) * (x118))) + (((cj4) * (x119))) +
1403 (((IkReal(-1.00000000000000)) * (new_r22) * (x115))));
1404 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1405 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
1406 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001)
1413 IkReal dummyeval[1];
1417 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
1420 IkReal dummyeval[1];
1422 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
1425 IkReal dummyeval[1];
1427 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
1430 IkReal evalcond[11];
1431 IkReal x120 = ((IkReal(-1.00000000000000)) + (new_r22));
1432 IkReal x121 = ((((cj3) * (new_r12))) +
1433 (((IkReal(-1.00000000000000)) * (new_r02) * (sj3))));
1434 IkReal x122 = ((((new_r12) * (sj3))) + (((cj3) * (new_r02))));
1436 ((IkReal(-3.14159265358979)) +
1437 (
IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
1439 evalcond[2] = new_r20;
1440 evalcond[3] = new_r21;
1444 evalcond[7] = new_r20;
1445 evalcond[8] = new_r21;
1447 evalcond[10] = x122;
1448 if (
IKabs(evalcond[0]) < 0.0000010000000000 &&
1449 IKabs(evalcond[1]) < 0.0000010000000000 &&
1450 IKabs(evalcond[2]) < 0.0000010000000000 &&
1451 IKabs(evalcond[3]) < 0.0000010000000000 &&
1452 IKabs(evalcond[4]) < 0.0000010000000000 &&
1453 IKabs(evalcond[5]) < 0.0000010000000000 &&
1454 IKabs(evalcond[6]) < 0.0000010000000000 &&
1455 IKabs(evalcond[7]) < 0.0000010000000000 &&
1456 IKabs(evalcond[8]) < 0.0000010000000000 &&
1457 IKabs(evalcond[9]) < 0.0000010000000000 &&
1458 IKabs(evalcond[10]) < 0.0000010000000000)
1461 IkReal j5array[1], cj5array[1], sj5array[1];
1462 bool j5valid[1] = {
false };
1464 if (
IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1465 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
1467 IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) <
1469 IKabs(
IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1470 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) +
1471 IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - 1) <=
1474 j5array[0] =
IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1475 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))),
1476 ((((new_r10) * (sj3))) + (((cj3) * (new_r00)))));
1477 sj5array[0] =
IKsin(j5array[0]);
1478 cj5array[0] =
IKcos(j5array[0]);
1479 if (j5array[0] >
IKPI)
1481 j5array[0] -=
IK2PI;
1483 else if (j5array[0] < -
IKPI)
1485 j5array[0] +=
IK2PI;
1488 for (
int ij5 = 0; ij5 < 1; ++ij5)
1496 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1498 if (j5valid[iij5] &&
1502 j5valid[iij5] =
false;
1508 cj5 = cj5array[ij5];
1509 sj5 = sj5array[ij5];
1512 IkReal x123 =
IKsin(j5);
1513 IkReal x124 = ((IkReal(1.00000000000000)) * (sj3));
1514 IkReal x125 = ((IkReal(1.00000000000000)) * (
IKcos(j5)));
1515 evalcond[0] = ((((IkReal(-1.00000000000000)) * (x123))) +
1516 (((IkReal(-1.00000000000000)) * (new_r00) * (x124))) +
1517 (((cj3) * (new_r10))));
1519 ((((IkReal(-1.00000000000000)) * (new_r01) * (x124))) +
1520 (((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x125))));
1521 evalcond[2] = ((((new_r11) * (sj3))) + (x123) + (((cj3) * (new_r01))));
1523 ((((new_r10) * (sj3))) + (((IkReal(-1.00000000000000)) * (x125))) +
1524 (((cj3) * (new_r00))));
1525 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1526 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001)
1533 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1534 vinfos[0].jointtype = 1;
1535 vinfos[0].foffset = j0;
1536 vinfos[0].indices[0] = _ij0[0];
1537 vinfos[0].indices[1] = _ij0[1];
1538 vinfos[0].maxsolutions = _nj0;
1539 vinfos[1].jointtype = 1;
1540 vinfos[1].foffset = j1;
1541 vinfos[1].indices[0] = _ij1[0];
1542 vinfos[1].indices[1] = _ij1[1];
1543 vinfos[1].maxsolutions = _nj1;
1544 vinfos[2].jointtype = 1;
1545 vinfos[2].foffset = j2;
1546 vinfos[2].indices[0] = _ij2[0];
1547 vinfos[2].indices[1] = _ij2[1];
1548 vinfos[2].maxsolutions = _nj2;
1549 vinfos[3].jointtype = 1;
1550 vinfos[3].foffset = j3;
1551 vinfos[3].indices[0] = _ij3[0];
1552 vinfos[3].indices[1] = _ij3[1];
1553 vinfos[3].maxsolutions = _nj3;
1554 vinfos[4].jointtype = 1;
1555 vinfos[4].foffset = j4;
1556 vinfos[4].indices[0] = _ij4[0];
1557 vinfos[4].indices[1] = _ij4[1];
1558 vinfos[4].maxsolutions = _nj4;
1559 vinfos[5].jointtype = 1;
1560 vinfos[5].foffset = j5;
1561 vinfos[5].indices[0] = _ij5[0];
1562 vinfos[5].indices[1] = _ij5[1];
1563 vinfos[5].maxsolutions = _nj5;
1564 std::vector<int> vfree(0);
1572 IkReal x126 = ((new_r12) * (sj3));
1573 IkReal x127 = ((IkReal(1.00000000000000)) * (new_r02));
1574 IkReal x128 = ((((IkReal(-1.00000000000000)) * (sj3) * (x127))) +
1575 (((cj3) * (new_r12))));
1577 ((IkReal(-3.14159265358979)) + (
IKfmod(j4, IkReal(6.28318530717959))));
1578 evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22));
1579 evalcond[2] = new_r20;
1580 evalcond[3] = new_r21;
1583 evalcond[6] = ((x126) + (((cj3) * (new_r02))));
1584 evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20));
1585 evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21));
1586 evalcond[9] = ((IkReal(-1.00000000000000)) +
1587 (((IkReal(-1.00000000000000)) * (new_r22))));
1588 evalcond[10] = ((((IkReal(-1.00000000000000)) * (cj3) * (x127))) +
1589 (((IkReal(-1.00000000000000)) * (x126))));
1590 if (
IKabs(evalcond[0]) < 0.0000010000000000 &&
1591 IKabs(evalcond[1]) < 0.0000010000000000 &&
1592 IKabs(evalcond[2]) < 0.0000010000000000 &&
1593 IKabs(evalcond[3]) < 0.0000010000000000 &&
1594 IKabs(evalcond[4]) < 0.0000010000000000 &&
1595 IKabs(evalcond[5]) < 0.0000010000000000 &&
1596 IKabs(evalcond[6]) < 0.0000010000000000 &&
1597 IKabs(evalcond[7]) < 0.0000010000000000 &&
1598 IKabs(evalcond[8]) < 0.0000010000000000 &&
1599 IKabs(evalcond[9]) < 0.0000010000000000 &&
1600 IKabs(evalcond[10]) < 0.0000010000000000)
1603 IkReal j5array[1], cj5array[1], sj5array[1];
1604 bool j5valid[1] = {
false };
1606 if (
IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) <
1608 IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1609 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
1611 IKabs(
IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
1612 IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1613 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
1617 IKatan2(((((new_r11) * (sj3))) + (((cj3) * (new_r01)))),
1618 ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1619 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3)))));
1620 sj5array[0] =
IKsin(j5array[0]);
1621 cj5array[0] =
IKcos(j5array[0]);
1622 if (j5array[0] >
IKPI)
1624 j5array[0] -=
IK2PI;
1626 else if (j5array[0] < -
IKPI)
1628 j5array[0] +=
IK2PI;
1631 for (
int ij5 = 0; ij5 < 1; ++ij5)
1639 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1641 if (j5valid[iij5] &&
1645 j5valid[iij5] =
false;
1651 cj5 = cj5array[ij5];
1652 sj5 = sj5array[ij5];
1655 IkReal x129 =
IKcos(j5);
1656 IkReal x130 = ((IkReal(1.00000000000000)) * (sj3));
1657 IkReal x131 = ((IkReal(1.00000000000000)) * (
IKsin(j5)));
1658 evalcond[0] = ((((IkReal(-1.00000000000000)) * (x131))) +
1659 (((IkReal(-1.00000000000000)) * (new_r00) * (x130))) +
1660 (((cj3) * (new_r10))));
1661 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x129))) +
1662 (((cj3) * (new_r11))) +
1663 (((IkReal(-1.00000000000000)) * (new_r01) * (x130))));
1664 evalcond[2] = ((((IkReal(-1.00000000000000)) * (x131))) +
1665 (((new_r11) * (sj3))) + (((cj3) * (new_r01))));
1667 ((x129) + (((new_r10) * (sj3))) + (((cj3) * (new_r00))));
1668 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1669 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001)
1676 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1677 vinfos[0].jointtype = 1;
1678 vinfos[0].foffset = j0;
1679 vinfos[0].indices[0] = _ij0[0];
1680 vinfos[0].indices[1] = _ij0[1];
1681 vinfos[0].maxsolutions = _nj0;
1682 vinfos[1].jointtype = 1;
1683 vinfos[1].foffset = j1;
1684 vinfos[1].indices[0] = _ij1[0];
1685 vinfos[1].indices[1] = _ij1[1];
1686 vinfos[1].maxsolutions = _nj1;
1687 vinfos[2].jointtype = 1;
1688 vinfos[2].foffset = j2;
1689 vinfos[2].indices[0] = _ij2[0];
1690 vinfos[2].indices[1] = _ij2[1];
1691 vinfos[2].maxsolutions = _nj2;
1692 vinfos[3].jointtype = 1;
1693 vinfos[3].foffset = j3;
1694 vinfos[3].indices[0] = _ij3[0];
1695 vinfos[3].indices[1] = _ij3[1];
1696 vinfos[3].maxsolutions = _nj3;
1697 vinfos[4].jointtype = 1;
1698 vinfos[4].foffset = j4;
1699 vinfos[4].indices[0] = _ij4[0];
1700 vinfos[4].indices[1] = _ij4[1];
1701 vinfos[4].maxsolutions = _nj4;
1702 vinfos[5].jointtype = 1;
1703 vinfos[5].foffset = j5;
1704 vinfos[5].indices[0] = _ij5[0];
1705 vinfos[5].indices[1] = _ij5[1];
1706 vinfos[5].maxsolutions = _nj5;
1707 std::vector<int> vfree(0);
1729 IkReal j5array[1], cj5array[1], sj5array[1];
1730 bool j5valid[1] = {
false };
1732 if (
IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1734 IKabs(((IkReal(-1.00000000000000)) * (new_r20) *
1735 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
1738 IKsqr(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1739 (((cj3) * (new_r10))))) +
1740 IKsqr(((IkReal(-1.00000000000000)) * (new_r20) *
1741 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) -
1745 IKatan2(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1746 (((cj3) * (new_r10)))),
1747 ((IkReal(-1.00000000000000)) * (new_r20) *
1748 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))));
1749 sj5array[0] =
IKsin(j5array[0]);
1750 cj5array[0] =
IKcos(j5array[0]);
1751 if (j5array[0] >
IKPI)
1753 j5array[0] -=
IK2PI;
1755 else if (j5array[0] < -
IKPI)
1757 j5array[0] +=
IK2PI;
1760 for (
int ij5 = 0; ij5 < 1; ++ij5)
1768 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1770 if (j5valid[iij5] &&
1774 j5valid[iij5] =
false;
1780 cj5 = cj5array[ij5];
1781 sj5 = sj5array[ij5];
1784 IkReal x132 =
IKsin(j5);
1785 IkReal x133 =
IKcos(j5);
1786 IkReal x134 = ((IkReal(1.00000000000000)) * (sj3));
1787 IkReal x135 = ((new_r11) * (sj3));
1788 IkReal x136 = ((new_r10) * (sj3));
1789 IkReal x137 = ((cj3) * (cj4));
1790 IkReal x138 = ((IkReal(1.00000000000000)) * (sj4));
1791 IkReal x139 = ((IkReal(1.00000000000000)) * (x133));
1792 IkReal x140 = ((IkReal(1.00000000000000)) * (x132));
1793 evalcond[0] = ((new_r20) + (((sj4) * (x133))));
1795 ((((IkReal(-1.00000000000000)) * (x132) * (x138))) + (new_r21));
1797 ((((IkReal(-1.00000000000000)) * (x140))) + (((cj3) * (new_r10))) +
1798 (((IkReal(-1.00000000000000)) * (new_r00) * (x134))));
1800 ((((IkReal(-1.00000000000000)) * (x139))) + (((cj3) * (new_r11))) +
1801 (((IkReal(-1.00000000000000)) * (new_r01) * (x134))));
1802 evalcond[4] = ((((cj4) * (x132))) + (x135) + (((cj3) * (new_r01))));
1803 evalcond[5] = ((x136) + (((IkReal(-1.00000000000000)) * (cj4) * (x139))) +
1804 (((cj3) * (new_r00))));
1805 evalcond[6] = ((((cj4) * (x135))) + (((new_r01) * (x137))) + (x132) +
1806 (((IkReal(-1.00000000000000)) * (new_r21) * (x138))));
1807 evalcond[7] = ((((IkReal(-1.00000000000000)) * (x139))) +
1808 (((IkReal(-1.00000000000000)) * (new_r20) * (x138))) +
1809 (((cj4) * (x136))) + (((new_r00) * (x137))));
1810 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1811 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
1812 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
1813 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001)
1820 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1821 vinfos[0].jointtype = 1;
1822 vinfos[0].foffset = j0;
1823 vinfos[0].indices[0] = _ij0[0];
1824 vinfos[0].indices[1] = _ij0[1];
1825 vinfos[0].maxsolutions = _nj0;
1826 vinfos[1].jointtype = 1;
1827 vinfos[1].foffset = j1;
1828 vinfos[1].indices[0] = _ij1[0];
1829 vinfos[1].indices[1] = _ij1[1];
1830 vinfos[1].maxsolutions = _nj1;
1831 vinfos[2].jointtype = 1;
1832 vinfos[2].foffset = j2;
1833 vinfos[2].indices[0] = _ij2[0];
1834 vinfos[2].indices[1] = _ij2[1];
1835 vinfos[2].maxsolutions = _nj2;
1836 vinfos[3].jointtype = 1;
1837 vinfos[3].foffset = j3;
1838 vinfos[3].indices[0] = _ij3[0];
1839 vinfos[3].indices[1] = _ij3[1];
1840 vinfos[3].maxsolutions = _nj3;
1841 vinfos[4].jointtype = 1;
1842 vinfos[4].foffset = j4;
1843 vinfos[4].indices[0] = _ij4[0];
1844 vinfos[4].indices[1] = _ij4[1];
1845 vinfos[4].maxsolutions = _nj4;
1846 vinfos[5].jointtype = 1;
1847 vinfos[5].foffset = j5;
1848 vinfos[5].indices[0] = _ij5[0];
1849 vinfos[5].indices[1] = _ij5[1];
1850 vinfos[5].maxsolutions = _nj5;
1851 std::vector<int> vfree(0);
1862 IkReal j5array[1], cj5array[1], sj5array[1];
1863 bool j5valid[1] = {
false };
1865 if (
IKabs(((new_r21) *
1866 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
1868 IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
1871 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) +
1872 IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
1873 (((cj3) * (new_r11))))) -
1877 ((new_r21) * (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))),
1878 ((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + (((cj3) * (new_r11)))));
1879 sj5array[0] =
IKsin(j5array[0]);
1880 cj5array[0] =
IKcos(j5array[0]);
1881 if (j5array[0] >
IKPI)
1883 j5array[0] -=
IK2PI;
1885 else if (j5array[0] < -
IKPI)
1887 j5array[0] +=
IK2PI;
1890 for (
int ij5 = 0; ij5 < 1; ++ij5)
1898 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1900 if (j5valid[iij5] &&
1904 j5valid[iij5] =
false;
1910 cj5 = cj5array[ij5];
1911 sj5 = sj5array[ij5];
1914 IkReal x141 =
IKsin(j5);
1915 IkReal x142 =
IKcos(j5);
1916 IkReal x143 = ((IkReal(1.00000000000000)) * (sj3));
1917 IkReal x144 = ((new_r11) * (sj3));
1918 IkReal x145 = ((new_r10) * (sj3));
1919 IkReal x146 = ((cj3) * (cj4));
1920 IkReal x147 = ((IkReal(1.00000000000000)) * (sj4));
1921 IkReal x148 = ((IkReal(1.00000000000000)) * (x142));
1922 IkReal x149 = ((IkReal(1.00000000000000)) * (x141));
1923 evalcond[0] = ((((sj4) * (x142))) + (new_r20));
1924 evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x141) * (x147))));
1926 ((((IkReal(-1.00000000000000)) * (new_r00) * (x143))) +
1927 (((cj3) * (new_r10))) + (((IkReal(-1.00000000000000)) * (x149))));
1928 evalcond[3] = ((((IkReal(-1.00000000000000)) * (x148))) +
1929 (((IkReal(-1.00000000000000)) * (new_r01) * (x143))) +
1930 (((cj3) * (new_r11))));
1931 evalcond[4] = ((((cj4) * (x141))) + (x144) + (((cj3) * (new_r01))));
1932 evalcond[5] = ((((IkReal(-1.00000000000000)) * (cj4) * (x148))) + (x145) +
1933 (((cj3) * (new_r00))));
1934 evalcond[6] = ((((cj4) * (x144))) + (((new_r01) * (x146))) + (x141) +
1935 (((IkReal(-1.00000000000000)) * (new_r21) * (x147))));
1936 evalcond[7] = ((((cj4) * (x145))) + (((IkReal(-1.00000000000000)) * (x148))) +
1937 (((IkReal(-1.00000000000000)) * (new_r20) * (x147))) +
1938 (((new_r00) * (x146))));
1939 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
1940 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
1941 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
1942 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001)
1949 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1950 vinfos[0].jointtype = 1;
1951 vinfos[0].foffset = j0;
1952 vinfos[0].indices[0] = _ij0[0];
1953 vinfos[0].indices[1] = _ij0[1];
1954 vinfos[0].maxsolutions = _nj0;
1955 vinfos[1].jointtype = 1;
1956 vinfos[1].foffset = j1;
1957 vinfos[1].indices[0] = _ij1[0];
1958 vinfos[1].indices[1] = _ij1[1];
1959 vinfos[1].maxsolutions = _nj1;
1960 vinfos[2].jointtype = 1;
1961 vinfos[2].foffset = j2;
1962 vinfos[2].indices[0] = _ij2[0];
1963 vinfos[2].indices[1] = _ij2[1];
1964 vinfos[2].maxsolutions = _nj2;
1965 vinfos[3].jointtype = 1;
1966 vinfos[3].foffset = j3;
1967 vinfos[3].indices[0] = _ij3[0];
1968 vinfos[3].indices[1] = _ij3[1];
1969 vinfos[3].maxsolutions = _nj3;
1970 vinfos[4].jointtype = 1;
1971 vinfos[4].foffset = j4;
1972 vinfos[4].indices[0] = _ij4[0];
1973 vinfos[4].indices[1] = _ij4[1];
1974 vinfos[4].maxsolutions = _nj4;
1975 vinfos[5].jointtype = 1;
1976 vinfos[5].foffset = j5;
1977 vinfos[5].indices[0] = _ij5[0];
1978 vinfos[5].indices[1] = _ij5[1];
1979 vinfos[5].maxsolutions = _nj5;
1980 std::vector<int> vfree(0);
1991 IkReal j5array[1], cj5array[1], sj5array[1];
1992 bool j5valid[1] = {
false };
1995 IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) <
1998 j5array[0] =
IKatan2(((gconst5) * (new_r21)),
1999 ((IkReal(-1.00000000000000)) * (gconst5) * (new_r20)));
2000 sj5array[0] =
IKsin(j5array[0]);
2001 cj5array[0] =
IKcos(j5array[0]);
2002 if (j5array[0] >
IKPI)
2004 j5array[0] -=
IK2PI;
2006 else if (j5array[0] < -
IKPI)
2008 j5array[0] +=
IK2PI;
2011 for (
int ij5 = 0; ij5 < 1; ++ij5)
2019 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2021 if (j5valid[iij5] &&
2025 j5valid[iij5] =
false;
2031 cj5 = cj5array[ij5];
2032 sj5 = sj5array[ij5];
2035 IkReal x150 =
IKsin(j5);
2036 IkReal x151 =
IKcos(j5);
2037 IkReal x152 = ((IkReal(1.00000000000000)) * (sj3));
2038 IkReal x153 = ((new_r11) * (sj3));
2039 IkReal x154 = ((new_r10) * (sj3));
2040 IkReal x155 = ((cj3) * (cj4));
2041 IkReal x156 = ((IkReal(1.00000000000000)) * (sj4));
2042 IkReal x157 = ((IkReal(1.00000000000000)) * (x151));
2043 IkReal x158 = ((IkReal(1.00000000000000)) * (x150));
2044 evalcond[0] = ((((sj4) * (x151))) + (new_r20));
2045 evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x150) * (x156))));
2046 evalcond[2] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x152))) +
2047 (((IkReal(-1.00000000000000)) * (x158))) + (((cj3) * (new_r10))));
2048 evalcond[3] = ((((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x157))) +
2049 (((IkReal(-1.00000000000000)) * (new_r01) * (x152))));
2050 evalcond[4] = ((x153) + (((cj4) * (x150))) + (((cj3) * (new_r01))));
2051 evalcond[5] = ((x154) + (((IkReal(-1.00000000000000)) * (cj4) * (x157))) +
2052 (((cj3) * (new_r00))));
2053 evalcond[6] = ((((IkReal(-1.00000000000000)) * (new_r21) * (x156))) + (x150) +
2054 (((new_r01) * (x155))) + (((cj4) * (x153))));
2055 evalcond[7] = ((((IkReal(-1.00000000000000)) * (x157))) +
2056 (((IkReal(-1.00000000000000)) * (new_r20) * (x156))) +
2057 (((cj4) * (x154))) + (((new_r00) * (x155))));
2058 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2059 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
2060 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
2061 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001)
2068 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2069 vinfos[0].jointtype = 1;
2070 vinfos[0].foffset = j0;
2071 vinfos[0].indices[0] = _ij0[0];
2072 vinfos[0].indices[1] = _ij0[1];
2073 vinfos[0].maxsolutions = _nj0;
2074 vinfos[1].jointtype = 1;
2075 vinfos[1].foffset = j1;
2076 vinfos[1].indices[0] = _ij1[0];
2077 vinfos[1].indices[1] = _ij1[1];
2078 vinfos[1].maxsolutions = _nj1;
2079 vinfos[2].jointtype = 1;
2080 vinfos[2].foffset = j2;
2081 vinfos[2].indices[0] = _ij2[0];
2082 vinfos[2].indices[1] = _ij2[1];
2083 vinfos[2].maxsolutions = _nj2;
2084 vinfos[3].jointtype = 1;
2085 vinfos[3].foffset = j3;
2086 vinfos[3].indices[0] = _ij3[0];
2087 vinfos[3].indices[1] = _ij3[1];
2088 vinfos[3].maxsolutions = _nj3;
2089 vinfos[4].jointtype = 1;
2090 vinfos[4].foffset = j4;
2091 vinfos[4].indices[0] = _ij4[0];
2092 vinfos[4].indices[1] = _ij4[1];
2093 vinfos[4].maxsolutions = _nj4;
2094 vinfos[5].jointtype = 1;
2095 vinfos[5].foffset = j5;
2096 vinfos[5].indices[0] = _ij5[0];
2097 vinfos[5].indices[1] = _ij5[1];
2098 vinfos[5].maxsolutions = _nj5;
2099 std::vector<int> vfree(0);
2114 IkReal j3array[1], cj3array[1], sj3array[1];
2115 bool j3valid[1] = {
false };
2117 IkReal x159 = ((gconst2) * (sj4));
2121 j3array[0] =
IKatan2(((new_r12) * (x159)), ((new_r02) * (x159)));
2122 sj3array[0] =
IKsin(j3array[0]);
2123 cj3array[0] =
IKcos(j3array[0]);
2124 if (j3array[0] >
IKPI)
2126 j3array[0] -=
IK2PI;
2128 else if (j3array[0] < -
IKPI)
2130 j3array[0] +=
IK2PI;
2133 for (
int ij3 = 0; ij3 < 1; ++ij3)
2141 for (
int iij3 = ij3 + 1; iij3 < 1; ++iij3)
2146 j3valid[iij3] =
false;
2152 cj3 = cj3array[ij3];
2153 sj3 = sj3array[ij3];
2156 IkReal x160 =
IKsin(j3);
2157 IkReal x161 =
IKcos(j3);
2158 IkReal x162 = ((IkReal(1.00000000000000)) * (sj4));
2159 IkReal x163 = ((sj4) * (x160));
2160 IkReal x164 = ((sj4) * (x161));
2161 IkReal x165 = ((new_r02) * (x161));
2162 IkReal x166 = ((new_r12) * (x160));
2163 evalcond[0] = ((((new_r12) * (x161))) + (((IkReal(-1.00000000000000)) * (new_r02) * (x160))));
2164 evalcond[1] = ((x166) + (x165) + (((IkReal(-1.00000000000000)) * (x162))));
2165 evalcond[2] = ((((new_r00) * (x164))) + (((new_r10) * (x163))) + (((cj4) * (new_r20))));
2166 evalcond[3] = ((((new_r01) * (x164))) + (((new_r11) * (x163))) + (((cj4) * (new_r21))));
2167 evalcond[4] = ((IkReal(-1.00000000000000)) + (((new_r02) * (x164))) + (((new_r12) * (x163))) +
2168 (((cj4) * (new_r22))));
2169 evalcond[5] = ((((cj4) * (x165))) + (((IkReal(-1.00000000000000)) * (new_r22) * (x162))) +
2170 (((cj4) * (x166))));
2171 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2172 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
2173 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001)
2180 IkReal dummyeval[1];
2184 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
2187 IkReal dummyeval[1];
2189 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
2192 IkReal dummyeval[1];
2194 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
2197 IkReal evalcond[11];
2198 IkReal x167 = ((IkReal(-1.00000000000000)) + (new_r22));
2200 ((((cj3) * (new_r12))) + (((IkReal(-1.00000000000000)) * (new_r02) * (sj3))));
2201 IkReal x169 = ((((new_r12) * (sj3))) + (((cj3) * (new_r02))));
2203 ((IkReal(-3.14159265358979)) +
2204 (
IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
2206 evalcond[2] = new_r20;
2207 evalcond[3] = new_r21;
2211 evalcond[7] = new_r20;
2212 evalcond[8] = new_r21;
2214 evalcond[10] = x169;
2215 if (
IKabs(evalcond[0]) < 0.0000010000000000 &&
2216 IKabs(evalcond[1]) < 0.0000010000000000 &&
2217 IKabs(evalcond[2]) < 0.0000010000000000 &&
2218 IKabs(evalcond[3]) < 0.0000010000000000 &&
2219 IKabs(evalcond[4]) < 0.0000010000000000 &&
2220 IKabs(evalcond[5]) < 0.0000010000000000 &&
2221 IKabs(evalcond[6]) < 0.0000010000000000 &&
2222 IKabs(evalcond[7]) < 0.0000010000000000 &&
2223 IKabs(evalcond[8]) < 0.0000010000000000 &&
2224 IKabs(evalcond[9]) < 0.0000010000000000 &&
2225 IKabs(evalcond[10]) < 0.0000010000000000)
2228 IkReal j5array[1], cj5array[1], sj5array[1];
2229 bool j5valid[1] = {
false };
2231 if (
IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2232 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
2234 IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) <
2236 IKabs(
IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2237 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) +
2238 IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - 1) <=
2241 j5array[0] =
IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2242 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))),
2243 ((((new_r10) * (sj3))) + (((cj3) * (new_r00)))));
2244 sj5array[0] =
IKsin(j5array[0]);
2245 cj5array[0] =
IKcos(j5array[0]);
2246 if (j5array[0] >
IKPI)
2248 j5array[0] -=
IK2PI;
2250 else if (j5array[0] < -
IKPI)
2252 j5array[0] +=
IK2PI;
2255 for (
int ij5 = 0; ij5 < 1; ++ij5)
2263 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2265 if (j5valid[iij5] &&
2269 j5valid[iij5] =
false;
2275 cj5 = cj5array[ij5];
2276 sj5 = sj5array[ij5];
2279 IkReal x170 =
IKsin(j5);
2280 IkReal x171 = ((IkReal(1.00000000000000)) * (sj3));
2281 IkReal x172 = ((IkReal(1.00000000000000)) * (
IKcos(j5)));
2283 ((((IkReal(-1.00000000000000)) * (x170))) + (((cj3) * (new_r10))) +
2284 (((IkReal(-1.00000000000000)) * (new_r00) * (x171))));
2286 ((((IkReal(-1.00000000000000)) * (new_r01) * (x171))) +
2287 (((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x172))));
2288 evalcond[2] = ((x170) + (((new_r11) * (sj3))) + (((cj3) * (new_r01))));
2290 ((((new_r10) * (sj3))) + (((IkReal(-1.00000000000000)) * (x172))) +
2291 (((cj3) * (new_r00))));
2292 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2293 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001)
2300 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2301 vinfos[0].jointtype = 1;
2302 vinfos[0].foffset = j0;
2303 vinfos[0].indices[0] = _ij0[0];
2304 vinfos[0].indices[1] = _ij0[1];
2305 vinfos[0].maxsolutions = _nj0;
2306 vinfos[1].jointtype = 1;
2307 vinfos[1].foffset = j1;
2308 vinfos[1].indices[0] = _ij1[0];
2309 vinfos[1].indices[1] = _ij1[1];
2310 vinfos[1].maxsolutions = _nj1;
2311 vinfos[2].jointtype = 1;
2312 vinfos[2].foffset = j2;
2313 vinfos[2].indices[0] = _ij2[0];
2314 vinfos[2].indices[1] = _ij2[1];
2315 vinfos[2].maxsolutions = _nj2;
2316 vinfos[3].jointtype = 1;
2317 vinfos[3].foffset = j3;
2318 vinfos[3].indices[0] = _ij3[0];
2319 vinfos[3].indices[1] = _ij3[1];
2320 vinfos[3].maxsolutions = _nj3;
2321 vinfos[4].jointtype = 1;
2322 vinfos[4].foffset = j4;
2323 vinfos[4].indices[0] = _ij4[0];
2324 vinfos[4].indices[1] = _ij4[1];
2325 vinfos[4].maxsolutions = _nj4;
2326 vinfos[5].jointtype = 1;
2327 vinfos[5].foffset = j5;
2328 vinfos[5].indices[0] = _ij5[0];
2329 vinfos[5].indices[1] = _ij5[1];
2330 vinfos[5].maxsolutions = _nj5;
2331 std::vector<int> vfree(0);
2339 IkReal x173 = ((new_r12) * (sj3));
2340 IkReal x174 = ((IkReal(1.00000000000000)) * (new_r02));
2342 ((((cj3) * (new_r12))) + (((IkReal(-1.00000000000000)) * (sj3) * (x174))));
2344 ((IkReal(-3.14159265358979)) + (
IKfmod(j4, IkReal(6.28318530717959))));
2345 evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22));
2346 evalcond[2] = new_r20;
2347 evalcond[3] = new_r21;
2350 evalcond[6] = ((x173) + (((cj3) * (new_r02))));
2351 evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20));
2352 evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21));
2354 ((IkReal(-1.00000000000000)) + (((IkReal(-1.00000000000000)) * (new_r22))));
2355 evalcond[10] = ((((IkReal(-1.00000000000000)) * (x173))) +
2356 (((IkReal(-1.00000000000000)) * (cj3) * (x174))));
2357 if (
IKabs(evalcond[0]) < 0.0000010000000000 &&
2358 IKabs(evalcond[1]) < 0.0000010000000000 &&
2359 IKabs(evalcond[2]) < 0.0000010000000000 &&
2360 IKabs(evalcond[3]) < 0.0000010000000000 &&
2361 IKabs(evalcond[4]) < 0.0000010000000000 &&
2362 IKabs(evalcond[5]) < 0.0000010000000000 &&
2363 IKabs(evalcond[6]) < 0.0000010000000000 &&
2364 IKabs(evalcond[7]) < 0.0000010000000000 &&
2365 IKabs(evalcond[8]) < 0.0000010000000000 &&
2366 IKabs(evalcond[9]) < 0.0000010000000000 &&
2367 IKabs(evalcond[10]) < 0.0000010000000000)
2370 IkReal j5array[1], cj5array[1], sj5array[1];
2371 bool j5valid[1] = {
false };
2373 if (
IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) <
2375 IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2376 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
2378 IKabs(
IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
2379 IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2380 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
2383 j5array[0] =
IKatan2(((((new_r11) * (sj3))) + (((cj3) * (new_r01)))),
2384 ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2385 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3)))));
2386 sj5array[0] =
IKsin(j5array[0]);
2387 cj5array[0] =
IKcos(j5array[0]);
2388 if (j5array[0] >
IKPI)
2390 j5array[0] -=
IK2PI;
2392 else if (j5array[0] < -
IKPI)
2394 j5array[0] +=
IK2PI;
2397 for (
int ij5 = 0; ij5 < 1; ++ij5)
2405 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2407 if (j5valid[iij5] &&
2411 j5valid[iij5] =
false;
2417 cj5 = cj5array[ij5];
2418 sj5 = sj5array[ij5];
2421 IkReal x176 =
IKcos(j5);
2422 IkReal x177 = ((IkReal(1.00000000000000)) * (sj3));
2423 IkReal x178 = ((IkReal(1.00000000000000)) * (
IKsin(j5)));
2425 ((((IkReal(-1.00000000000000)) * (new_r00) * (x177))) +
2426 (((IkReal(-1.00000000000000)) * (x178))) + (((cj3) * (new_r10))));
2427 evalcond[1] = ((((cj3) * (new_r11))) +
2428 (((IkReal(-1.00000000000000)) * (new_r01) * (x177))) +
2429 (((IkReal(-1.00000000000000)) * (x176))));
2430 evalcond[2] = ((((IkReal(-1.00000000000000)) * (x178))) +
2431 (((new_r11) * (sj3))) + (((cj3) * (new_r01))));
2432 evalcond[3] = ((x176) + (((new_r10) * (sj3))) + (((cj3) * (new_r00))));
2433 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2434 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001)
2441 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2442 vinfos[0].jointtype = 1;
2443 vinfos[0].foffset = j0;
2444 vinfos[0].indices[0] = _ij0[0];
2445 vinfos[0].indices[1] = _ij0[1];
2446 vinfos[0].maxsolutions = _nj0;
2447 vinfos[1].jointtype = 1;
2448 vinfos[1].foffset = j1;
2449 vinfos[1].indices[0] = _ij1[0];
2450 vinfos[1].indices[1] = _ij1[1];
2451 vinfos[1].maxsolutions = _nj1;
2452 vinfos[2].jointtype = 1;
2453 vinfos[2].foffset = j2;
2454 vinfos[2].indices[0] = _ij2[0];
2455 vinfos[2].indices[1] = _ij2[1];
2456 vinfos[2].maxsolutions = _nj2;
2457 vinfos[3].jointtype = 1;
2458 vinfos[3].foffset = j3;
2459 vinfos[3].indices[0] = _ij3[0];
2460 vinfos[3].indices[1] = _ij3[1];
2461 vinfos[3].maxsolutions = _nj3;
2462 vinfos[4].jointtype = 1;
2463 vinfos[4].foffset = j4;
2464 vinfos[4].indices[0] = _ij4[0];
2465 vinfos[4].indices[1] = _ij4[1];
2466 vinfos[4].maxsolutions = _nj4;
2467 vinfos[5].jointtype = 1;
2468 vinfos[5].foffset = j5;
2469 vinfos[5].indices[0] = _ij5[0];
2470 vinfos[5].indices[1] = _ij5[1];
2471 vinfos[5].maxsolutions = _nj5;
2472 std::vector<int> vfree(0);
2494 IkReal j5array[1], cj5array[1], sj5array[1];
2495 bool j5valid[1] = {
false };
2497 if (
IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
2499 IKabs(((IkReal(-1.00000000000000)) * (new_r20) *
2500 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
2502 IKabs(
IKsqr(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
2503 (((cj3) * (new_r10))))) +
2504 IKsqr(((IkReal(-1.00000000000000)) * (new_r20) *
2505 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) -
2509 ((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) + (((cj3) * (new_r10)))),
2510 ((IkReal(-1.00000000000000)) * (new_r20) *
2511 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))));
2512 sj5array[0] =
IKsin(j5array[0]);
2513 cj5array[0] =
IKcos(j5array[0]);
2514 if (j5array[0] >
IKPI)
2516 j5array[0] -=
IK2PI;
2518 else if (j5array[0] < -
IKPI)
2520 j5array[0] +=
IK2PI;
2523 for (
int ij5 = 0; ij5 < 1; ++ij5)
2531 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2533 if (j5valid[iij5] &&
2537 j5valid[iij5] =
false;
2543 cj5 = cj5array[ij5];
2544 sj5 = sj5array[ij5];
2547 IkReal x179 =
IKsin(j5);
2548 IkReal x180 =
IKcos(j5);
2549 IkReal x181 = ((IkReal(1.00000000000000)) * (sj3));
2550 IkReal x182 = ((new_r11) * (sj3));
2551 IkReal x183 = ((new_r10) * (sj3));
2552 IkReal x184 = ((cj3) * (cj4));
2553 IkReal x185 = ((IkReal(1.00000000000000)) * (sj4));
2554 IkReal x186 = ((IkReal(1.00000000000000)) * (x180));
2555 IkReal x187 = ((IkReal(1.00000000000000)) * (x179));
2556 evalcond[0] = ((((sj4) * (x180))) + (new_r20));
2557 evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x179) * (x185))));
2559 ((((IkReal(-1.00000000000000)) * (new_r00) * (x181))) +
2560 (((IkReal(-1.00000000000000)) * (x187))) + (((cj3) * (new_r10))));
2562 ((((IkReal(-1.00000000000000)) * (new_r01) * (x181))) +
2563 (((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (x186))));
2564 evalcond[4] = ((x182) + (((cj3) * (new_r01))) + (((cj4) * (x179))));
2565 evalcond[5] = ((x183) + (((IkReal(-1.00000000000000)) * (cj4) * (x186))) +
2566 (((cj3) * (new_r00))));
2567 evalcond[6] = ((x179) + (((cj4) * (x182))) + (((new_r01) * (x184))) +
2568 (((IkReal(-1.00000000000000)) * (new_r21) * (x185))));
2569 evalcond[7] = ((((new_r00) * (x184))) + (((cj4) * (x183))) +
2570 (((IkReal(-1.00000000000000)) * (new_r20) * (x185))) +
2571 (((IkReal(-1.00000000000000)) * (x186))));
2572 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2573 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
2574 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
2575 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001)
2582 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2583 vinfos[0].jointtype = 1;
2584 vinfos[0].foffset = j0;
2585 vinfos[0].indices[0] = _ij0[0];
2586 vinfos[0].indices[1] = _ij0[1];
2587 vinfos[0].maxsolutions = _nj0;
2588 vinfos[1].jointtype = 1;
2589 vinfos[1].foffset = j1;
2590 vinfos[1].indices[0] = _ij1[0];
2591 vinfos[1].indices[1] = _ij1[1];
2592 vinfos[1].maxsolutions = _nj1;
2593 vinfos[2].jointtype = 1;
2594 vinfos[2].foffset = j2;
2595 vinfos[2].indices[0] = _ij2[0];
2596 vinfos[2].indices[1] = _ij2[1];
2597 vinfos[2].maxsolutions = _nj2;
2598 vinfos[3].jointtype = 1;
2599 vinfos[3].foffset = j3;
2600 vinfos[3].indices[0] = _ij3[0];
2601 vinfos[3].indices[1] = _ij3[1];
2602 vinfos[3].maxsolutions = _nj3;
2603 vinfos[4].jointtype = 1;
2604 vinfos[4].foffset = j4;
2605 vinfos[4].indices[0] = _ij4[0];
2606 vinfos[4].indices[1] = _ij4[1];
2607 vinfos[4].maxsolutions = _nj4;
2608 vinfos[5].jointtype = 1;
2609 vinfos[5].foffset = j5;
2610 vinfos[5].indices[0] = _ij5[0];
2611 vinfos[5].indices[1] = _ij5[1];
2612 vinfos[5].maxsolutions = _nj5;
2613 std::vector<int> vfree(0);
2624 IkReal j5array[1], cj5array[1], sj5array[1];
2625 bool j5valid[1] = {
false };
2627 if (
IKabs(((new_r21) * (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
2629 IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
2632 (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) +
2633 IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
2634 (((cj3) * (new_r11))))) -
2638 ((new_r21) * (((
IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30))),
2639 ((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + (((cj3) * (new_r11)))));
2640 sj5array[0] =
IKsin(j5array[0]);
2641 cj5array[0] =
IKcos(j5array[0]);
2642 if (j5array[0] >
IKPI)
2644 j5array[0] -=
IK2PI;
2646 else if (j5array[0] < -
IKPI)
2648 j5array[0] +=
IK2PI;
2651 for (
int ij5 = 0; ij5 < 1; ++ij5)
2659 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2661 if (j5valid[iij5] &&
2665 j5valid[iij5] =
false;
2671 cj5 = cj5array[ij5];
2672 sj5 = sj5array[ij5];
2675 IkReal x188 =
IKsin(j5);
2676 IkReal x189 =
IKcos(j5);
2677 IkReal x190 = ((IkReal(1.00000000000000)) * (sj3));
2678 IkReal x191 = ((new_r11) * (sj3));
2679 IkReal x192 = ((new_r10) * (sj3));
2680 IkReal x193 = ((cj3) * (cj4));
2681 IkReal x194 = ((IkReal(1.00000000000000)) * (sj4));
2682 IkReal x195 = ((IkReal(1.00000000000000)) * (x189));
2683 IkReal x196 = ((IkReal(1.00000000000000)) * (x188));
2684 evalcond[0] = ((new_r20) + (((sj4) * (x189))));
2685 evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x188) * (x194))));
2687 ((((IkReal(-1.00000000000000)) * (x196))) +
2688 (((IkReal(-1.00000000000000)) * (new_r00) * (x190))) + (((cj3) * (new_r10))));
2690 ((((cj3) * (new_r11))) + (((IkReal(-1.00000000000000)) * (new_r01) * (x190))) +
2691 (((IkReal(-1.00000000000000)) * (x195))));
2692 evalcond[4] = ((((cj4) * (x188))) + (x191) + (((cj3) * (new_r01))));
2693 evalcond[5] = ((((IkReal(-1.00000000000000)) * (cj4) * (x195))) + (x192) +
2694 (((cj3) * (new_r00))));
2695 evalcond[6] = ((((IkReal(-1.00000000000000)) * (new_r21) * (x194))) +
2696 (((cj4) * (x191))) + (x188) + (((new_r01) * (x193))));
2698 ((((new_r00) * (x193))) + (((IkReal(-1.00000000000000)) * (x195))) +
2699 (((cj4) * (x192))) + (((IkReal(-1.00000000000000)) * (new_r20) * (x194))));
2700 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2701 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
2702 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
2703 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001)
2710 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2711 vinfos[0].jointtype = 1;
2712 vinfos[0].foffset = j0;
2713 vinfos[0].indices[0] = _ij0[0];
2714 vinfos[0].indices[1] = _ij0[1];
2715 vinfos[0].maxsolutions = _nj0;
2716 vinfos[1].jointtype = 1;
2717 vinfos[1].foffset = j1;
2718 vinfos[1].indices[0] = _ij1[0];
2719 vinfos[1].indices[1] = _ij1[1];
2720 vinfos[1].maxsolutions = _nj1;
2721 vinfos[2].jointtype = 1;
2722 vinfos[2].foffset = j2;
2723 vinfos[2].indices[0] = _ij2[0];
2724 vinfos[2].indices[1] = _ij2[1];
2725 vinfos[2].maxsolutions = _nj2;
2726 vinfos[3].jointtype = 1;
2727 vinfos[3].foffset = j3;
2728 vinfos[3].indices[0] = _ij3[0];
2729 vinfos[3].indices[1] = _ij3[1];
2730 vinfos[3].maxsolutions = _nj3;
2731 vinfos[4].jointtype = 1;
2732 vinfos[4].foffset = j4;
2733 vinfos[4].indices[0] = _ij4[0];
2734 vinfos[4].indices[1] = _ij4[1];
2735 vinfos[4].maxsolutions = _nj4;
2736 vinfos[5].jointtype = 1;
2737 vinfos[5].foffset = j5;
2738 vinfos[5].indices[0] = _ij5[0];
2739 vinfos[5].indices[1] = _ij5[1];
2740 vinfos[5].maxsolutions = _nj5;
2741 std::vector<int> vfree(0);
2752 IkReal j5array[1], cj5array[1], sj5array[1];
2753 bool j5valid[1] = {
false };
2759 IKatan2(((gconst5) * (new_r21)), ((IkReal(-1.00000000000000)) * (gconst5) * (new_r20)));
2760 sj5array[0] =
IKsin(j5array[0]);
2761 cj5array[0] =
IKcos(j5array[0]);
2762 if (j5array[0] >
IKPI)
2764 j5array[0] -=
IK2PI;
2766 else if (j5array[0] < -
IKPI)
2768 j5array[0] +=
IK2PI;
2771 for (
int ij5 = 0; ij5 < 1; ++ij5)
2779 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2784 j5valid[iij5] =
false;
2790 cj5 = cj5array[ij5];
2791 sj5 = sj5array[ij5];
2794 IkReal x197 =
IKsin(j5);
2795 IkReal x198 =
IKcos(j5);
2796 IkReal x199 = ((IkReal(1.00000000000000)) * (sj3));
2797 IkReal x200 = ((new_r11) * (sj3));
2798 IkReal x201 = ((new_r10) * (sj3));
2799 IkReal x202 = ((cj3) * (cj4));
2800 IkReal x203 = ((IkReal(1.00000000000000)) * (sj4));
2801 IkReal x204 = ((IkReal(1.00000000000000)) * (x198));
2802 IkReal x205 = ((IkReal(1.00000000000000)) * (x197));
2803 evalcond[0] = ((new_r20) + (((sj4) * (x198))));
2804 evalcond[1] = ((new_r21) + (((IkReal(-1.00000000000000)) * (x197) * (x203))));
2805 evalcond[2] = ((((IkReal(-1.00000000000000)) * (x205))) + (((cj3) * (new_r10))) +
2806 (((IkReal(-1.00000000000000)) * (new_r00) * (x199))));
2807 evalcond[3] = ((((IkReal(-1.00000000000000)) * (x204))) + (((cj3) * (new_r11))) +
2808 (((IkReal(-1.00000000000000)) * (new_r01) * (x199))));
2809 evalcond[4] = ((((cj4) * (x197))) + (x200) + (((cj3) * (new_r01))));
2811 ((x201) + (((IkReal(-1.00000000000000)) * (cj4) * (x204))) + (((cj3) * (new_r00))));
2812 evalcond[6] = ((((new_r01) * (x202))) + (((cj4) * (x200))) + (x197) +
2813 (((IkReal(-1.00000000000000)) * (new_r21) * (x203))));
2814 evalcond[7] = ((((cj4) * (x201))) + (((new_r00) * (x202))) +
2815 (((IkReal(-1.00000000000000)) * (x204))) +
2816 (((IkReal(-1.00000000000000)) * (new_r20) * (x203))));
2817 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
2818 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
2819 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
2820 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001)
2827 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2828 vinfos[0].jointtype = 1;
2829 vinfos[0].foffset = j0;
2830 vinfos[0].indices[0] = _ij0[0];
2831 vinfos[0].indices[1] = _ij0[1];
2832 vinfos[0].maxsolutions = _nj0;
2833 vinfos[1].jointtype = 1;
2834 vinfos[1].foffset = j1;
2835 vinfos[1].indices[0] = _ij1[0];
2836 vinfos[1].indices[1] = _ij1[1];
2837 vinfos[1].maxsolutions = _nj1;
2838 vinfos[2].jointtype = 1;
2839 vinfos[2].foffset = j2;
2840 vinfos[2].indices[0] = _ij2[0];
2841 vinfos[2].indices[1] = _ij2[1];
2842 vinfos[2].maxsolutions = _nj2;
2843 vinfos[3].jointtype = 1;
2844 vinfos[3].foffset = j3;
2845 vinfos[3].indices[0] = _ij3[0];
2846 vinfos[3].indices[1] = _ij3[1];
2847 vinfos[3].maxsolutions = _nj3;
2848 vinfos[4].jointtype = 1;
2849 vinfos[4].foffset = j4;
2850 vinfos[4].indices[0] = _ij4[0];
2851 vinfos[4].indices[1] = _ij4[1];
2852 vinfos[4].maxsolutions = _nj4;
2853 vinfos[5].jointtype = 1;
2854 vinfos[5].foffset = j5;
2855 vinfos[5].indices[0] = _ij5[0];
2856 vinfos[5].indices[1] = _ij5[1];
2857 vinfos[5].maxsolutions = _nj5;
2858 std::vector<int> vfree(0);
2873 IkReal j5array[1], cj5array[1], sj5array[1];
2874 bool j5valid[1] = {
false };
2879 j5array[0] =
IKatan2(((gconst4) * (new_r21)), ((IkReal(-1.00000000000000)) * (gconst4) * (new_r20)));
2880 sj5array[0] =
IKsin(j5array[0]);
2881 cj5array[0] =
IKcos(j5array[0]);
2882 if (j5array[0] >
IKPI)
2884 j5array[0] -=
IK2PI;
2886 else if (j5array[0] < -
IKPI)
2888 j5array[0] +=
IK2PI;
2891 for (
int ij5 = 0; ij5 < 1; ++ij5)
2899 for (
int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2904 j5valid[iij5] =
false;
2910 cj5 = cj5array[ij5];
2911 sj5 = sj5array[ij5];
2914 evalcond[0] = ((new_r20) + (((sj4) * (
IKcos(j5)))));
2915 evalcond[1] = ((((IkReal(-1.00000000000000)) * (sj4) * (
IKsin(j5)))) + (new_r21));
2916 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001)
2923 IkReal dummyeval[1];
2925 gconst6 =
IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02))));
2926 dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02)));
2927 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
2930 IkReal dummyeval[1];
2932 gconst7 =
IKsign(((((IkReal(-1.00000000000000)) * (new_r01) * (new_r02))) +
2933 (((IkReal(-1.00000000000000)) * (new_r11) * (new_r12)))));
2934 dummyeval[0] = ((((IkReal(-1.00000000000000)) * (new_r01) * (new_r02))) +
2935 (((IkReal(-1.00000000000000)) * (new_r11) * (new_r12))));
2936 if (
IKabs(dummyeval[0]) < 0.0000010000000000)
2943 IkReal j3array[1], cj3array[1], sj3array[1];
2944 bool j3valid[1] = {
false };
2946 IkReal x206 = ((cj4) * (gconst7) * (sj5));
2950 j3array[0] =
IKatan2(((new_r12) * (x206)), ((new_r02) * (x206)));
2951 sj3array[0] =
IKsin(j3array[0]);
2952 cj3array[0] =
IKcos(j3array[0]);
2953 if (j3array[0] >
IKPI)
2955 j3array[0] -=
IK2PI;
2957 else if (j3array[0] < -
IKPI)
2959 j3array[0] +=
IK2PI;
2962 for (
int ij3 = 0; ij3 < 1; ++ij3)
2970 for (
int iij3 = ij3 + 1; iij3 < 1; ++iij3)
2975 j3valid[iij3] =
false;
2981 cj3 = cj3array[ij3];
2982 sj3 = sj3array[ij3];
2984 IkReal evalcond[12];
2985 IkReal x207 =
IKsin(j3);
2986 IkReal x208 =
IKcos(j3);
2987 IkReal x209 = ((IkReal(1.00000000000000)) * (cj5));
2988 IkReal x210 = ((IkReal(1.00000000000000)) * (sj4));
2989 IkReal x211 = ((cj4) * (x208));
2990 IkReal x212 = ((sj4) * (x208));
2991 IkReal x213 = ((cj4) * (x207));
2992 IkReal x214 = ((new_r11) * (x207));
2993 IkReal x215 = ((sj4) * (x207));
2994 IkReal x216 = ((IkReal(1.00000000000000)) * (x207));
2996 ((((IkReal(-1.00000000000000)) * (new_r02) * (x216))) + (((new_r12) * (x208))));
2997 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x210))) + (((new_r12) * (x207))) +
2998 (((new_r02) * (x208))));
3000 ((((new_r10) * (x208))) + (((IkReal(-1.00000000000000)) * (new_r00) * (x216))) +
3001 (((IkReal(-1.00000000000000)) * (sj5))));
3002 evalcond[3] = ((((IkReal(-1.00000000000000)) * (new_r01) * (x216))) +
3003 (((IkReal(-1.00000000000000)) * (x209))) + (((new_r11) * (x208))));
3004 evalcond[4] = ((((new_r01) * (x208))) + (x214) + (((cj4) * (sj5))));
3005 evalcond[5] = ((((new_r10) * (x207))) + (((new_r00) * (x208))) +
3006 (((IkReal(-1.00000000000000)) * (cj4) * (x209))));
3007 evalcond[6] = ((((new_r00) * (x212))) + (((new_r10) * (x215))) + (((cj4) * (new_r20))));
3008 evalcond[7] = ((((cj4) * (new_r21))) + (((new_r01) * (x212))) + (((sj4) * (x214))));
3009 evalcond[8] = ((IkReal(-1.00000000000000)) + (((cj4) * (new_r22))) +
3010 (((new_r02) * (x212))) + (((new_r12) * (x215))));
3011 evalcond[9] = ((((new_r12) * (x213))) + (((new_r02) * (x211))) +
3012 (((IkReal(-1.00000000000000)) * (new_r22) * (x210))));
3014 ((((new_r11) * (x213))) + (((IkReal(-1.00000000000000)) * (new_r21) * (x210))) +
3015 (sj5) + (((new_r01) * (x211))));
3017 ((((IkReal(-1.00000000000000)) * (x209))) + (((new_r00) * (x211))) +
3018 (((IkReal(-1.00000000000000)) * (new_r20) * (x210))) + (((new_r10) * (x213))));
3019 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
3020 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
3021 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
3022 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001 ||
3023 IKabs(evalcond[8]) > 0.000001 ||
IKabs(evalcond[9]) > 0.000001 ||
3024 IKabs(evalcond[10]) > 0.000001 ||
IKabs(evalcond[11]) > 0.000001)
3031 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3032 vinfos[0].jointtype = 1;
3033 vinfos[0].foffset = j0;
3034 vinfos[0].indices[0] = _ij0[0];
3035 vinfos[0].indices[1] = _ij0[1];
3036 vinfos[0].maxsolutions = _nj0;
3037 vinfos[1].jointtype = 1;
3038 vinfos[1].foffset = j1;
3039 vinfos[1].indices[0] = _ij1[0];
3040 vinfos[1].indices[1] = _ij1[1];
3041 vinfos[1].maxsolutions = _nj1;
3042 vinfos[2].jointtype = 1;
3043 vinfos[2].foffset = j2;
3044 vinfos[2].indices[0] = _ij2[0];
3045 vinfos[2].indices[1] = _ij2[1];
3046 vinfos[2].maxsolutions = _nj2;
3047 vinfos[3].jointtype = 1;
3048 vinfos[3].foffset = j3;
3049 vinfos[3].indices[0] = _ij3[0];
3050 vinfos[3].indices[1] = _ij3[1];
3051 vinfos[3].maxsolutions = _nj3;
3052 vinfos[4].jointtype = 1;
3053 vinfos[4].foffset = j4;
3054 vinfos[4].indices[0] = _ij4[0];
3055 vinfos[4].indices[1] = _ij4[1];
3056 vinfos[4].maxsolutions = _nj4;
3057 vinfos[5].jointtype = 1;
3058 vinfos[5].foffset = j5;
3059 vinfos[5].indices[0] = _ij5[0];
3060 vinfos[5].indices[1] = _ij5[1];
3061 vinfos[5].maxsolutions = _nj5;
3062 std::vector<int> vfree(0);
3073 IkReal j3array[1], cj3array[1], sj3array[1];
3074 bool j3valid[1] = {
false };
3076 IkReal x217 = ((gconst6) * (sj4));
3080 j3array[0] =
IKatan2(((new_r12) * (x217)), ((new_r02) * (x217)));
3081 sj3array[0] =
IKsin(j3array[0]);
3082 cj3array[0] =
IKcos(j3array[0]);
3083 if (j3array[0] >
IKPI)
3085 j3array[0] -=
IK2PI;
3087 else if (j3array[0] < -
IKPI)
3089 j3array[0] +=
IK2PI;
3092 for (
int ij3 = 0; ij3 < 1; ++ij3)
3100 for (
int iij3 = ij3 + 1; iij3 < 1; ++iij3)
3105 j3valid[iij3] =
false;
3111 cj3 = cj3array[ij3];
3112 sj3 = sj3array[ij3];
3114 IkReal evalcond[12];
3115 IkReal x218 =
IKsin(j3);
3116 IkReal x219 =
IKcos(j3);
3117 IkReal x220 = ((IkReal(1.00000000000000)) * (cj5));
3118 IkReal x221 = ((IkReal(1.00000000000000)) * (sj4));
3119 IkReal x222 = ((cj4) * (x219));
3120 IkReal x223 = ((sj4) * (x219));
3121 IkReal x224 = ((cj4) * (x218));
3122 IkReal x225 = ((new_r11) * (x218));
3123 IkReal x226 = ((sj4) * (x218));
3124 IkReal x227 = ((IkReal(1.00000000000000)) * (x218));
3126 ((((new_r12) * (x219))) + (((IkReal(-1.00000000000000)) * (new_r02) * (x227))));
3127 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x221))) + (((new_r12) * (x218))) +
3128 (((new_r02) * (x219))));
3129 evalcond[2] = ((((IkReal(-1.00000000000000)) * (new_r00) * (x227))) +
3130 (((new_r10) * (x219))) + (((IkReal(-1.00000000000000)) * (sj5))));
3131 evalcond[3] = ((((IkReal(-1.00000000000000)) * (x220))) + (((new_r11) * (x219))) +
3132 (((IkReal(-1.00000000000000)) * (new_r01) * (x227))));
3133 evalcond[4] = ((((new_r01) * (x219))) + (x225) + (((cj4) * (sj5))));
3134 evalcond[5] = ((((new_r00) * (x219))) + (((new_r10) * (x218))) +
3135 (((IkReal(-1.00000000000000)) * (cj4) * (x220))));
3136 evalcond[6] = ((((new_r00) * (x223))) + (((cj4) * (new_r20))) + (((new_r10) * (x226))));
3137 evalcond[7] = ((((sj4) * (x225))) + (((cj4) * (new_r21))) + (((new_r01) * (x223))));
3138 evalcond[8] = ((IkReal(-1.00000000000000)) + (((new_r12) * (x226))) +
3139 (((new_r02) * (x223))) + (((cj4) * (new_r22))));
3140 evalcond[9] = ((((IkReal(-1.00000000000000)) * (new_r22) * (x221))) +
3141 (((new_r12) * (x224))) + (((new_r02) * (x222))));
3142 evalcond[10] = ((((new_r11) * (x224))) + (((new_r01) * (x222))) + (sj5) +
3143 (((IkReal(-1.00000000000000)) * (new_r21) * (x221))));
3145 ((((IkReal(-1.00000000000000)) * (x220))) + (((new_r10) * (x224))) +
3146 (((IkReal(-1.00000000000000)) * (new_r20) * (x221))) + (((new_r00) * (x222))));
3147 if (
IKabs(evalcond[0]) > 0.000001 ||
IKabs(evalcond[1]) > 0.000001 ||
3148 IKabs(evalcond[2]) > 0.000001 ||
IKabs(evalcond[3]) > 0.000001 ||
3149 IKabs(evalcond[4]) > 0.000001 ||
IKabs(evalcond[5]) > 0.000001 ||
3150 IKabs(evalcond[6]) > 0.000001 ||
IKabs(evalcond[7]) > 0.000001 ||
3151 IKabs(evalcond[8]) > 0.000001 ||
IKabs(evalcond[9]) > 0.000001 ||
3152 IKabs(evalcond[10]) > 0.000001 ||
IKabs(evalcond[11]) > 0.000001)
3159 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3160 vinfos[0].jointtype = 1;
3161 vinfos[0].foffset = j0;
3162 vinfos[0].indices[0] = _ij0[0];
3163 vinfos[0].indices[1] = _ij0[1];
3164 vinfos[0].maxsolutions = _nj0;
3165 vinfos[1].jointtype = 1;
3166 vinfos[1].foffset = j1;
3167 vinfos[1].indices[0] = _ij1[0];
3168 vinfos[1].indices[1] = _ij1[1];
3169 vinfos[1].maxsolutions = _nj1;
3170 vinfos[2].jointtype = 1;
3171 vinfos[2].foffset = j2;
3172 vinfos[2].indices[0] = _ij2[0];
3173 vinfos[2].indices[1] = _ij2[1];
3174 vinfos[2].maxsolutions = _nj2;
3175 vinfos[3].jointtype = 1;
3176 vinfos[3].foffset = j3;
3177 vinfos[3].indices[0] = _ij3[0];
3178 vinfos[3].indices[1] = _ij3[1];
3179 vinfos[3].maxsolutions = _nj3;
3180 vinfos[4].jointtype = 1;
3181 vinfos[4].foffset = j4;
3182 vinfos[4].indices[0] = _ij4[0];
3183 vinfos[4].indices[1] = _ij4[1];
3184 vinfos[4].maxsolutions = _nj4;
3185 vinfos[5].jointtype = 1;
3186 vinfos[5].foffset = j5;
3187 vinfos[5].indices[0] = _ij5[0];
3188 vinfos[5].indices[1] = _ij5[1];
3189 vinfos[5].maxsolutions = _nj5;
3190 std::vector<int> vfree(0);
3213 return solver.
ComputeIk(eetrans, eerot, pfree, solutions);
3218 return "<robot:genericrobot - abb_irb2400 (1f04c8a90b29778d31a8f2cb88b4a166)>";
3223#ifdef IKFAST_NAMESPACE
3227#ifndef IKFAST_NO_MAIN
3230#ifdef IKFAST_NAMESPACE
3231using namespace IKFAST_NAMESPACE;
3233int main(
int argc,
char** argv)
3237 printf(
"\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
3238 "Returns the ik solutions given the transformation of the end effector specified by\n"
3239 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
3240 "There are %d free parameters that have to be specified.\n\n",
3247 IkReal eerot[9], eetrans[3];
3248 eerot[0] = atof(argv[1]);
3249 eerot[1] = atof(argv[2]);
3250 eerot[2] = atof(argv[3]);
3251 eetrans[0] = atof(argv[4]);
3252 eerot[3] = atof(argv[5]);
3253 eerot[4] = atof(argv[6]);
3254 eerot[5] = atof(argv[7]);
3255 eetrans[1] = atof(argv[8]);
3256 eerot[6] = atof(argv[9]);
3257 eerot[7] = atof(argv[10]);
3258 eerot[8] = atof(argv[11]);
3259 eetrans[2] = atof(argv[12]);
3260 for (std::size_t i = 0; i < vfree.size(); ++i)
3261 vfree[i] = atof(argv[13 + i]);
3262 bool bSuccess =
ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
3266 fprintf(stderr,
"Failed to get ik solution\n");
3275 printf(
"sol%d (free=%d): ", (
int)i, (
int)sol.
GetFree().size());
3276 std::vector<IkReal> vsolfree(sol.
GetFree().size());
3277 sol.
GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
3278 for (std::size_t
j = 0;
j < solvalues.size(); ++
j)
3279 printf(
"%.15f, ", solvalues[
j]);
3283 IkReal rot[9], trans[3];
3284 IkReal sol[6] = { 0.46365, 0.93285, 1.75595 - M_PI / 2.0, 6.28319, -2.68880, -0.46365 };
3286 printf(
"FK: %f %f %f\n", trans[0], trans[1], trans[2]);
#define IKPI
Definition: abb_irb2400_ikfast_solver.hpp:79
#define IKFAST_ATAN2_MAGTHRESH
Definition: abb_irb2400_ikfast_solver.hpp:142
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
#define IKFAST_COMPILE_ASSERT(x)
Definition: abb_irb2400_ikfast_solver.hpp:32
float IKasin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:150
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 int * GetFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:380
IKFAST_API int GetNumJoints()
Definition: abb_irb2400_ikfast_solver.hpp:381
#define IKPI_2
Definition: abb_irb2400_ikfast_solver.hpp:80
IKFAST_API const char * GetIkFastVersion()
Definition: abb_irb2400_ikfast_solver.hpp:3221
float IKatan2(float fy, float fx)
Definition: abb_irb2400_ikfast_solver.hpp:229
#define IKFAST_SINCOS_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:137
IKFAST_API int GetIkRealSize()
Definition: abb_irb2400_ikfast_solver.hpp:383
float IKacos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:191
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKFAST_API const char * GetKinematicsHash()
Definition: abb_irb2400_ikfast_solver.hpp:3216
float IKsqrt(float f)
Definition: abb_irb2400_ikfast_solver.hpp:217
float IKsin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:211
float IKsign(float f)
Definition: abb_irb2400_ikfast_solver.hpp:256
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
float IKfmod(float x, float y)
Definition: abb_irb2400_ikfast_solver.hpp:172
float IKcos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:213
float IKsqr(float f)
Definition: abb_irb2400_ikfast_solver.hpp:129
IKFAST_API int GetIkType()
Definition: abb_irb2400_ikfast_solver.hpp:385
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: abb_irb2400_ikfast_solver.hpp:284
float IKtan(float f)
Definition: abb_irb2400_ikfast_solver.hpp:215
#define IK2PI
Definition: abb_irb2400_ikfast_solver.hpp:78
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:3210
#define IKFAST_SOLUTION_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:147
float IKlog(float f)
Definition: abb_irb2400_ikfast_solver.hpp:132
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)
#define IKFAST_STRINGIZE(s)
Definition: abb_irb2400_ikfast_solver.hpp:41
IKFAST_API int GetNumFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:379
#define IKFAST_ASSERT(b)
Definition: abb_irb2400_ikfast_solver.hpp:59
float IKabs(float f)
Definition: abb_irb2400_ikfast_solver.hpp:126
Definition: abb_irb2400_ikfast_solver.hpp:388
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:867
IkReal new_px
Definition: abb_irb2400_ikfast_solver.hpp:392
IkReal new_py
Definition: abb_irb2400_ikfast_solver.hpp:393
IkReal cj0
Definition: abb_irb2400_ikfast_solver.hpp:390
IkReal htj5
Definition: abb_irb2400_ikfast_solver.hpp:391
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:396
The discrete solutions are returned in this structure.
Definition: ikfast.h:72
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...
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
manages all the solutions
Definition: ikfast.h:103
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
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
Default implementation of IkSolutionListBase.
Definition: ikfast.h:259
const IkSolutionBase< T > & GetSolution(size_t index) const override
returns the solution pointer
Definition: ikfast.h:268
size_t GetNumSolutions() const override
returns the number of solutions stored
Definition: ikfast.h:279
int main(int argc, char **argv)
Definition: create_convex_hull.cpp:43
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:45
tesseract_kinematics::Manipulability m
Definition: kinematics_core_unit.cpp:179
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
Definition: create_convex_hull.cpp:36
JointDynamics j
Definition: tesseract_scene_graph_joint_unit.cpp:15
KinematicsInformation info
Definition: tesseract_srdf_unit.cpp:1765