Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
abb_irb2400_ikfast_solver.hpp
Go to the documentation of this file.
1// clang-format off
2
22#include <tesseract_common/macros.h>
23TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
24#ifndef IKFAST_HAS_LIBRARY
25#define IKFAST_HAS_LIBRARY
26#define IKFAST_NO_MAIN
27#endif
28#include <tesseract_kinematics/ikfast/external/ikfast.h> // found inside share/openrave-X.Y/python/ikfast.h
29using namespace ikfast;
30
31// check if the included ikfast version matches what this file was compiled with
32#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)(x)]
33IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61);
34
35#include <cmath>
36#include <vector>
37#include <limits>
38#include <complex>
39
40#define IKFAST_STRINGIZE2(s) #s
41#define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
42
43#ifndef IKFAST_ASSERT
44#include <stdexcept>
45#include <iostream>
46#include <cstddef> // for NULL
47#include <memory> // for allocator_traits<>::value_type
48
49#ifdef _MSC_VER
50#ifndef __PRETTY_FUNCTION__
51#define __PRETTY_FUNCTION__ __FUNCDNAME__
52#endif
53#endif
54
55#ifndef __PRETTY_FUNCTION__
56#define __PRETTY_FUNCTION__ __func__
57#endif
58
59#define IKFAST_ASSERT(b) \
60 { \
61 if (!(b)) \
62 { \
63 std::stringstream ss; \
64 ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " << __PRETTY_FUNCTION__ << ": Assertion '" \
65 << #b << "' failed"; \
66 throw std::runtime_error(ss.str()); \
67 } \
68 }
69
70#endif
71
72#if defined(_MSC_VER)
73#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
74#else
75#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
76#endif
77
78#define IK2PI ((IkReal)6.28318530717959)
79#define IKPI ((IkReal)3.14159265358979)
80#define IKPI_2 ((IkReal)1.57079632679490)
81
82#ifdef _MSC_VER
83#ifndef isnan
84#define isnan _isnan
85#endif
86#endif // _MSC_VER
87
88// lapack routines
89extern "C" {
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);
94void dgetrs_(const char* trans,
95 const int* n,
96 const int* nrhs,
97 double* a,
98 const int* lda,
99 int* ipiv,
100 double* b,
101 const int* ldb,
102 int* info);
103void dgeev_(const char* jobvl,
104 const char* jobvr,
105 const int* n,
106 double* a,
107 const int* lda,
108 double* wr,
109 double* wi,
110 double* vl,
111 const int* ldvl,
112 double* vr,
113 const int* ldvr,
114 double* work,
115 const int* lwork,
116 int* info);
117}
118
119using namespace std; // necessary to get std math routines
120
121#ifdef IKFAST_NAMESPACE
122namespace IKFAST_NAMESPACE
123{
124#endif
125
126inline float IKabs(float f) { return fabsf(f); }
127inline double IKabs(double f) { return fabs(f); }
128
129inline float IKsqr(float f) { return f * f; }
130inline double IKsqr(double f) { return f * f; }
131
132inline float IKlog(float f) { return logf(f); }
133inline double IKlog(double f) { return log(f); }
134
135// allows asin and acos to exceed 1
136#ifndef IKFAST_SINCOS_THRESH
137#define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
138#endif
139
140// used to check input to atan2 for degenerate cases
141#ifndef IKFAST_ATAN2_MAGTHRESH
142#define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
143#endif
144
145// minimum distance of separate solutions
146#ifndef IKFAST_SOLUTION_THRESH
147#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
148#endif
149
150inline float IKasin(float f)
151{
152 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
153 // wrong with the solver
154 if (f <= -1)
155 return float(-IKPI_2);
156 else if (f >= 1)
157 return float(IKPI_2);
158 return asinf(f);
159}
160inline double IKasin(double f)
161{
162 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
163 // wrong with the solver
164 if (f <= -1)
165 return -IKPI_2;
166 else if (f >= 1)
167 return IKPI_2;
168 return asin(f);
169}
170
171// return positive value in [0,y)
172inline float IKfmod(float x, float y)
173{
174 while (x < 0)
175 {
176 x += y;
177 }
178 return fmodf(x, y);
179}
180
181// return positive value in [0,y)
182inline double IKfmod(double x, double y)
183{
184 while (x < 0)
185 {
186 x += y;
187 }
188 return fmod(x, y);
189}
190
191inline float IKacos(float f)
192{
193 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
194 // wrong with the solver
195 if (f <= -1)
196 return float(IKPI);
197 else if (f >= 1)
198 return float(0);
199 return acosf(f);
200}
201inline double IKacos(double f)
202{
203 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
204 // wrong with the solver
205 if (f <= -1)
206 return IKPI;
207 else if (f >= 1)
208 return 0;
209 return acos(f);
210}
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); }
217inline float IKsqrt(float f)
218{
219 if (f <= 0.0f)
220 return 0.0f;
221 return sqrtf(f);
222}
223inline double IKsqrt(double f)
224{
225 if (f <= 0.0)
226 return 0.0;
227 return sqrt(f);
228}
229inline float IKatan2(float fy, float fx)
230{
231 if (isnan(fy))
232 {
233 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
234 return float(IKPI_2);
235 }
236 else if (isnan(fx))
237 {
238 return 0;
239 }
240 return atan2f(fy, fx);
241}
242inline double IKatan2(double fy, double fx)
243{
244 if (isnan(fy))
245 {
246 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
247 return IKPI_2;
248 }
249 else if (isnan(fx))
250 {
251 return 0;
252 }
253 return atan2(fy, fx);
254}
255
256inline float IKsign(float f)
257{
258 if (f > 0)
259 {
260 return float(1);
261 }
262 else if (f < 0)
263 {
264 return float(-1);
265 }
266 return 0;
267}
268
269inline double IKsign(double f)
270{
271 if (f > 0)
272 {
273 return 1.0;
274 }
275 else if (f < 0)
276 {
277 return -1.0;
278 }
279 return 0;
280}
281
284IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
285{
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,
288 x47, x48, x49;
289 x0 = IKcos(j[0]);
290 x1 = IKcos(j[3]);
291 x2 = IKsin(j[0]);
292 x3 = IKsin(j[3]);
293 x4 = IKsin(j[1]);
294 x5 = IKsin(j[2]);
295 x6 = IKcos(j[1]);
296 x7 = IKcos(j[2]);
297 x8 = IKsin(j[5]);
298 x9 = IKsin(j[4]);
299 x10 = IKcos(j[4]);
300 x11 = IKcos(j[5]);
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));
312 x23 = ((x0) * (x4));
313 x24 = ((x6) * (x7));
314 x25 = ((x5) * (x6));
315 x26 = ((x2) * (x4));
316 x27 = ((x4) * (x5));
317 x28 = ((x1) * (x9));
318 x29 = ((x4) * (x7));
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))))))));
340 eerot[1] =
341 ((((x8) *
342 (((((IkReal(-1.00000000000000)) * (x18) * (x39))) + (((IkReal(-1.00000000000000)) * (x19) * (x45))))))) +
343 (((x11) * (x46))));
344 eerot[2] = ((((x45) * (x9))) + (((x10) * (((((IkReal(-1.00000000000000)) * (x31))) + (((x0) * (x24))))))));
345 IkReal x50 = ((x0) * (x24));
346 IkReal x51 = ((IkReal(1.00000000000000)) * (x23));
347 eetrans[0] =
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));
360 eetrans[1] =
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));
371 eetrans[2] =
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))));
377}
378
379IKFAST_API int GetNumFreeParameters() { return 0; }
380IKFAST_API int* GetFreeParameters() { return NULL; }
381IKFAST_API int GetNumJoints() { return 6; }
382
383IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
384
385IKFAST_API int GetIkType() { return 0x67000001; }
386
387class IKSolver
388{
389public:
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,
393 new_py, py, npy, new_pz, pz, npz, pp;
394 unsigned char _ij0[2], _nj0, _ij1[2], _nj1, _ij2[2], _nj2, _ij3[2], _nj3, _ij4[2], _nj4, _ij5[2], _nj5;
395
396 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
397 {
398 j0 = numeric_limits<IkReal>::quiet_NaN();
399 _ij0[0] = -1;
400 _ij0[1] = -1;
401 _nj0 = -1;
402 j1 = numeric_limits<IkReal>::quiet_NaN();
403 _ij1[0] = -1;
404 _ij1[1] = -1;
405 _nj1 = -1;
406 j2 = numeric_limits<IkReal>::quiet_NaN();
407 _ij2[0] = -1;
408 _ij2[1] = -1;
409 _nj2 = -1;
410 j3 = numeric_limits<IkReal>::quiet_NaN();
411 _ij3[0] = -1;
412 _ij3[1] = -1;
413 _nj3 = -1;
414 j4 = numeric_limits<IkReal>::quiet_NaN();
415 _ij4[0] = -1;
416 _ij4[1] = -1;
417 _nj4 = -1;
418 j5 = numeric_limits<IkReal>::quiet_NaN();
419 _ij5[0] = -1;
420 _ij5[1] = -1;
421 _nj5 = -1;
422 for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
423 {
424 solutions.Clear();
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];
434 px = eetrans[0];
435 py = eetrans[1];
436 pz = eetrans[2];
437
438 new_r00 = r00;
439 new_r01 = r01;
440 new_r02 = r02;
441 new_px = ((((IkReal(-0.0850000000000000)) * (r02))) + (px));
442 new_r10 = r10;
443 new_r11 = r11;
444 new_r12 = r12;
445 new_py = ((((IkReal(-0.0850000000000000)) * (r12))) + (py));
446 new_r20 = r20;
447 new_r21 = r21;
448 new_r22 = r22;
449 new_pz = ((IkReal(-0.615000000000000)) + (((IkReal(-0.0850000000000000)) * (r22))) + (pz));
450 r00 = new_r00;
451 r01 = new_r01;
452 r02 = new_r02;
453 r10 = new_r10;
454 r11 = new_r11;
455 r12 = new_r12;
456 r20 = new_r20;
457 r21 = new_r21;
458 r22 = new_r22;
459 px = new_px;
460 py = new_py;
461 pz = new_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))));
475 {
476 IkReal j0array[2], cj0array[2], sj0array[2];
477 bool j0valid[2] = { false };
478 _nj0 = 2;
479 if (IKabs(((IkReal(-1.00000000000000)) * (py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH)
480 continue;
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)
489 {
490 j0array[0] -= IK2PI;
491 }
492 else if (j0array[0] < -IKPI)
493 {
494 j0array[0] += IK2PI;
495 }
496 j0valid[0] = true;
497 if (j0array[1] > IKPI)
498 {
499 j0array[1] -= IK2PI;
500 }
501 else if (j0array[1] < -IKPI)
502 {
503 j0array[1] += IK2PI;
504 }
505 j0valid[1] = true;
506 for (int ij0 = 0; ij0 < 2; ++ij0)
507 {
508 if (!j0valid[ij0])
509 {
510 continue;
511 }
512 _ij0[0] = ij0;
513 _ij0[1] = -1;
514 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
515 {
516 if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
517 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
518 {
519 j0valid[iij0] = false;
520 _ij0[1] = iij0;
521 break;
522 }
523 }
524 j0 = j0array[ij0];
525 cj0 = cj0array[ij0];
526 sj0 = sj0array[ij0];
527
528 {
529 IkReal j2array[2], cj2array[2], sj2array[2];
530 bool j2valid[2] = { false };
531 _nj2 = 2;
532 if ((((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
533 (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp))))) <
534 -1 - IKFAST_SINCOS_THRESH ||
535 (((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) +
536 (((IkReal(0.184939600473773)) * (cj0) * (px))) + (((IkReal(-0.924698002368864)) * (pp))))) >
537 1 + IKFAST_SINCOS_THRESH)
538 continue;
539 IkReal x56 =
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)
549 {
550 j2array[0] -= IK2PI;
551 }
552 else if (j2array[0] < -IKPI)
553 {
554 j2array[0] += IK2PI;
555 }
556 j2valid[0] = true;
557 if (j2array[1] > IKPI)
558 {
559 j2array[1] -= IK2PI;
560 }
561 else if (j2array[1] < -IKPI)
562 {
563 j2array[1] += IK2PI;
564 }
565 j2valid[1] = true;
566 for (int ij2 = 0; ij2 < 2; ++ij2)
567 {
568 if (!j2valid[ij2])
569 {
570 continue;
571 }
572 _ij2[0] = ij2;
573 _ij2[1] = -1;
574 for (int iij2 = ij2 + 1; iij2 < 2; ++iij2)
575 {
576 if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
577 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
578 {
579 j2valid[iij2] = false;
580 _ij2[1] = iij2;
581 break;
582 }
583 }
584 j2 = j2array[ij2];
585 cj2 = cj2array[ij2];
586 sj2 = sj2array[ij2];
587
588 {
589 IkReal dummyeval[1];
590 IkReal gconst1;
591 IkReal x57 = ((IkReal(0.755000000000000)) * (cj2));
592 IkReal x58 = ((py) * (sj0));
593 IkReal x59 = ((cj0) * (px));
594 IkReal x60 = ((IkReal(0.135000000000000)) * (sj2));
595 gconst1 = IKsign(
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));
605 dummyeval[0] =
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)
611 {
612 {
613 IkReal dummyeval[1];
614 IkReal gconst0;
615 IkReal x65 = ((IkReal(0.755000000000000)) * (sj2));
616 IkReal x66 = ((cj0) * (px));
617 IkReal x67 = ((py) * (sj0));
618 IkReal x68 = ((IkReal(0.135000000000000)) * (cj2));
619 gconst0 = IKsign(
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));
629 dummyeval[0] =
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))) +
635 (((x69) * (x71))));
636 if (IKabs(dummyeval[0]) < 0.0000010000000000)
637 {
638 continue;
639 }
640 else
641 {
642 {
643 IkReal j1array[1], cj1array[1], sj1array[1];
644 bool j1valid[1] = { false };
645 _nj1 = 1;
646 IkReal x73 = (sj2) * (sj2);
647 IkReal x74 = (cj2) * (cj2);
648 IkReal x75 = ((cj2) * (sj2));
649 IkReal x76 = ((IkReal(1.00000000000000)) * (pz));
650 if (IKabs(
651 ((gconst0) *
652 (((IkReal(-0.497025000000000)) + (((IkReal(-0.190350000000000)) * (cj2))) +
653 ((pz) * (pz)) + (((IkReal(1.06455000000000)) * (sj2))) +
654 (((IkReal(0.203850000000000)) * (x75))) + (((IkReal(-0.570025000000000)) * (x73))) +
655 (((IkReal(-0.0182250000000000)) * (x74))))))) < IKFAST_ATAN2_MAGTHRESH &&
656 IKabs((
657 (gconst0) *
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))) +
662 (((IkReal(-1.00000000000000)) * (cj0) * (px) * (x76))))))) < IKFAST_ATAN2_MAGTHRESH)
663 continue;
664 j1array[0] = IKatan2(
665 ((gconst0) *
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)))))),
669 ((gconst0) *
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)
678 {
679 j1array[0] -= IK2PI;
680 }
681 else if (j1array[0] < -IKPI)
682 {
683 j1array[0] += IK2PI;
684 }
685 j1valid[0] = true;
686 for (int ij1 = 0; ij1 < 1; ++ij1)
687 {
688 if (!j1valid[ij1])
689 {
690 continue;
691 }
692 _ij1[0] = ij1;
693 _ij1[1] = -1;
694 for (int iij1 = ij1 + 1; iij1 < 1; ++iij1)
695 {
696 if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
697 IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
698 {
699 j1valid[iij1] = false;
700 _ij1[1] = iij1;
701 break;
702 }
703 }
704 j1 = j1array[ij1];
705 cj1 = cj1array[ij1];
706 sj1 = sj1array[ij1];
707 {
708 IkReal evalcond[5];
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))));
730 evalcond[3] =
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))));
735 evalcond[4] =
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)
743 {
744 continue;
745 }
746 }
747
748 rotationfunction0(solutions);
749 }
750 }
751 }
752 }
753 }
754 else
755 {
756 {
757 IkReal j1array[1], cj1array[1], sj1array[1];
758 bool j1valid[1] = { false };
759 _nj1 = 1;
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))))))) <
768 IKFAST_ATAN2_MAGTHRESH &&
769 IKabs(((gconst1) * (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) +
770 (((IkReal(-0.203850000000000)) * (x230))) +
771 (((IkReal(-0.570025000000000)) * (x229))))))) < IKFAST_ATAN2_MAGTHRESH)
772 continue;
773 j1array[0] =
774 IKatan2(((gconst1) *
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)
785 {
786 j1array[0] -= IK2PI;
787 }
788 else if (j1array[0] < -IKPI)
789 {
790 j1array[0] += IK2PI;
791 }
792 j1valid[0] = true;
793 for (int ij1 = 0; ij1 < 1; ++ij1)
794 {
795 if (!j1valid[ij1])
796 {
797 continue;
798 }
799 _ij1[0] = ij1;
800 _ij1[1] = -1;
801 for (int iij1 = ij1 + 1; iij1 < 1; ++iij1)
802 {
803 if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
804 IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
805 {
806 j1valid[iij1] = false;
807 _ij1[1] = iij1;
808 break;
809 }
810 }
811 j1 = j1array[ij1];
812 cj1 = cj1array[ij1];
813 sj1 = sj1array[ij1];
814 {
815 IkReal evalcond[5];
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))));
834 evalcond[2] =
835 ((((IkReal(-0.705000000000000)) * (x232))) + (((x231) * (x237))) + (((x231) * (x233))) +
836 (pz) + (((x232) * (x235))) + (((IkReal(-1.00000000000000)) * (x232) * (x238))));
837 evalcond[3] =
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))));
842 evalcond[4] =
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)
850 {
851 continue;
852 }
853 }
854
855 rotationfunction0(solutions);
856 }
857 }
858 }
859 }
860 }
861 }
862 }
863 }
864 }
865 return solutions.GetNumSolutions() > 0;
866 }
867 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
868 {
869 for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
870 {
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))));
894 {
895 IkReal j4array[2], cj4array[2], sj4array[2];
896 bool j4valid[2] = { false };
897 _nj4 = 2;
898 cj4array[0] = new_r22;
899 if (cj4array[0] >= -1 - IKFAST_SINCOS_THRESH && cj4array[0] <= 1 + IKFAST_SINCOS_THRESH)
900 {
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];
907 }
908 else if (isnan(cj4array[0]))
909 {
910 // probably any value will work
911 j4valid[0] = true;
912 cj4array[0] = 1;
913 sj4array[0] = 0;
914 j4array[0] = 0;
915 }
916 for (int ij4 = 0; ij4 < 2; ++ij4)
917 {
918 if (!j4valid[ij4])
919 {
920 continue;
921 }
922 _ij4[0] = ij4;
923 _ij4[1] = -1;
924 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
925 {
926 if (j4valid[iij4] && IKabs(cj4array[ij4] - cj4array[iij4]) < IKFAST_SOLUTION_THRESH &&
927 IKabs(sj4array[ij4] - sj4array[iij4]) < IKFAST_SOLUTION_THRESH)
928 {
929 j4valid[iij4] = false;
930 _ij4[1] = iij4;
931 break;
932 }
933 }
934 j4 = j4array[ij4];
935 cj4 = cj4array[ij4];
936 sj4 = sj4array[ij4];
937
938 {
939 IkReal dummyeval[1];
940 IkReal gconst4;
941 gconst4 = IKsign(sj4);
942 dummyeval[0] = sj4;
943 if (IKabs(dummyeval[0]) < 0.0000010000000000)
944 {
945 {
946 IkReal dummyeval[1];
947 IkReal gconst2;
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)
951 {
952 {
953 IkReal dummyeval[1];
954 IkReal gconst3;
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)
958 {
959 {
960 IkReal evalcond[7];
961 IkReal x103 = ((IkReal(-1.00000000000000)) + (new_r22));
962 evalcond[0] = ((IkReal(-3.14159265358979)) +
963 (IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
964 evalcond[1] = x103;
965 evalcond[2] = new_r20;
966 evalcond[3] = new_r21;
967 evalcond[4] = new_r20;
968 evalcond[5] = new_r21;
969 evalcond[6] = x103;
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)
974 {
975 {
976 IkReal j3array[2], cj3array[2], sj3array[2];
977 bool j3valid[2] = { false };
978 _nj3 = 2;
979 if (IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH)
980 continue;
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)
989 {
990 j3array[0] -= IK2PI;
991 }
992 else if (j3array[0] < -IKPI)
993 {
994 j3array[0] += IK2PI;
995 }
996 j3valid[0] = true;
997 if (j3array[1] > IKPI)
998 {
999 j3array[1] -= IK2PI;
1000 }
1001 else if (j3array[1] < -IKPI)
1002 {
1003 j3array[1] += IK2PI;
1004 }
1005 j3valid[1] = true;
1006 for (int ij3 = 0; ij3 < 2; ++ij3)
1007 {
1008 if (!j3valid[ij3])
1009 {
1010 continue;
1011 }
1012 _ij3[0] = ij3;
1013 _ij3[1] = -1;
1014 for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
1015 {
1016 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
1017 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
1018 {
1019 j3valid[iij3] = false;
1020 _ij3[1] = iij3;
1021 break;
1022 }
1023 }
1024 j3 = j3array[ij3];
1025 cj3 = cj3array[ij3];
1026 sj3 = sj3array[ij3];
1027 {
1028 IkReal evalcond[1];
1029 evalcond[0] = ((((new_r12) * (IKcos(j3)))) +
1030 (((IkReal(-1.00000000000000)) * (new_r02) * (IKsin(j3)))));
1031 if (IKabs(evalcond[0]) > 0.000001)
1032 {
1033 continue;
1034 }
1035 }
1036
1037 {
1038 IkReal j5array[1], cj5array[1], sj5array[1];
1039 bool j5valid[1] = { false };
1040 _nj5 = 1;
1041 if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1042 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
1043 IKFAST_ATAN2_MAGTHRESH &&
1044 IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) < IKFAST_ATAN2_MAGTHRESH &&
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) <=
1048 IKFAST_SINCOS_THRESH)
1049 continue;
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)
1056 {
1057 j5array[0] -= IK2PI;
1058 }
1059 else if (j5array[0] < -IKPI)
1060 {
1061 j5array[0] += IK2PI;
1062 }
1063 j5valid[0] = true;
1064 for (int ij5 = 0; ij5 < 1; ++ij5)
1065 {
1066 if (!j5valid[ij5])
1067 {
1068 continue;
1069 }
1070 _ij5[0] = ij5;
1071 _ij5[1] = -1;
1072 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1073 {
1074 if (j5valid[iij5] &&
1075 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1076 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1077 {
1078 j5valid[iij5] = false;
1079 _ij5[1] = iij5;
1080 break;
1081 }
1082 }
1083 j5 = j5array[ij5];
1084 cj5 = cj5array[ij5];
1085 sj5 = sj5array[ij5];
1086 {
1087 IkReal evalcond[4];
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)
1100 {
1101 continue;
1102 }
1103 }
1104
1105 {
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);
1138 solutions.AddSolution(vinfos, vfree);
1139 }
1140 }
1141 }
1142 }
1143 }
1144 }
1145 else
1146 {
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)
1158 {
1159 {
1160 IkReal j3array[2], cj3array[2], sj3array[2];
1161 bool j3valid[2] = { false };
1162 _nj3 = 2;
1163 if (IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH)
1164 continue;
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)
1173 {
1174 j3array[0] -= IK2PI;
1175 }
1176 else if (j3array[0] < -IKPI)
1177 {
1178 j3array[0] += IK2PI;
1179 }
1180 j3valid[0] = true;
1181 if (j3array[1] > IKPI)
1182 {
1183 j3array[1] -= IK2PI;
1184 }
1185 else if (j3array[1] < -IKPI)
1186 {
1187 j3array[1] += IK2PI;
1188 }
1189 j3valid[1] = true;
1190 for (int ij3 = 0; ij3 < 2; ++ij3)
1191 {
1192 if (!j3valid[ij3])
1193 {
1194 continue;
1195 }
1196 _ij3[0] = ij3;
1197 _ij3[1] = -1;
1198 for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
1199 {
1200 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
1201 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
1202 {
1203 j3valid[iij3] = false;
1204 _ij3[1] = iij3;
1205 break;
1206 }
1207 }
1208 j3 = j3array[ij3];
1209 cj3 = cj3array[ij3];
1210 sj3 = sj3array[ij3];
1211 {
1212 IkReal evalcond[1];
1213 evalcond[0] = ((((new_r12) * (IKcos(j3)))) +
1214 (((IkReal(-1.00000000000000)) * (new_r02) * (IKsin(j3)))));
1215 if (IKabs(evalcond[0]) > 0.000001)
1216 {
1217 continue;
1218 }
1219 }
1220
1221 {
1222 IkReal j5array[1], cj5array[1], sj5array[1];
1223 bool j5valid[1] = { false };
1224 _nj5 = 1;
1225 if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) < IKFAST_ATAN2_MAGTHRESH &&
1226 IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1227 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
1228 IKFAST_ATAN2_MAGTHRESH &&
1229 IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
1230 IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1231 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
1232 1) <= IKFAST_SINCOS_THRESH)
1233 continue;
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)
1240 {
1241 j5array[0] -= IK2PI;
1242 }
1243 else if (j5array[0] < -IKPI)
1244 {
1245 j5array[0] += IK2PI;
1246 }
1247 j5valid[0] = true;
1248 for (int ij5 = 0; ij5 < 1; ++ij5)
1249 {
1250 if (!j5valid[ij5])
1251 {
1252 continue;
1253 }
1254 _ij5[0] = ij5;
1255 _ij5[1] = -1;
1256 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1257 {
1258 if (j5valid[iij5] &&
1259 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1260 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1261 {
1262 j5valid[iij5] = false;
1263 _ij5[1] = iij5;
1264 break;
1265 }
1266 }
1267 j5 = j5array[ij5];
1268 cj5 = cj5array[ij5];
1269 sj5 = sj5array[ij5];
1270 {
1271 IkReal evalcond[4];
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)
1285 {
1286 continue;
1287 }
1288 }
1289
1290 {
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);
1323 solutions.AddSolution(vinfos, vfree);
1324 }
1325 }
1326 }
1327 }
1328 }
1329 }
1330 else
1331 {
1332 if (1)
1333 {
1334 continue;
1335 }
1336 else
1337 {
1338 }
1339 }
1340 }
1341 }
1342 }
1343 else
1344 {
1345 {
1346 IkReal j3array[1], cj3array[1], sj3array[1];
1347 bool j3valid[1] = { false };
1348 _nj3 = 1;
1349 IkReal x112 = ((IkReal(-1.00000000000000)) * (cj4) * (gconst3) * (new_r20));
1350 if (IKabs(((new_r12) * (x112))) < IKFAST_ATAN2_MAGTHRESH &&
1351 IKabs(((new_r02) * (x112))) < IKFAST_ATAN2_MAGTHRESH)
1352 continue;
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)
1357 {
1358 j3array[0] -= IK2PI;
1359 }
1360 else if (j3array[0] < -IKPI)
1361 {
1362 j3array[0] += IK2PI;
1363 }
1364 j3valid[0] = true;
1365 for (int ij3 = 0; ij3 < 1; ++ij3)
1366 {
1367 if (!j3valid[ij3])
1368 {
1369 continue;
1370 }
1371 _ij3[0] = ij3;
1372 _ij3[1] = -1;
1373 for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
1374 {
1375 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
1376 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
1377 {
1378 j3valid[iij3] = false;
1379 _ij3[1] = iij3;
1380 break;
1381 }
1382 }
1383 j3 = j3array[ij3];
1384 cj3 = cj3array[ij3];
1385 sj3 = sj3array[ij3];
1386 {
1387 IkReal evalcond[6];
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));
1395 evalcond[0] =
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)
1407 {
1408 continue;
1409 }
1410 }
1411
1412 {
1413 IkReal dummyeval[1];
1414 IkReal gconst5;
1415 gconst5 = IKsign(sj4);
1416 dummyeval[0] = sj4;
1417 if (IKabs(dummyeval[0]) < 0.0000010000000000)
1418 {
1419 {
1420 IkReal dummyeval[1];
1421 dummyeval[0] = sj4;
1422 if (IKabs(dummyeval[0]) < 0.0000010000000000)
1423 {
1424 {
1425 IkReal dummyeval[1];
1426 dummyeval[0] = sj4;
1427 if (IKabs(dummyeval[0]) < 0.0000010000000000)
1428 {
1429 {
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))));
1435 evalcond[0] =
1436 ((IkReal(-3.14159265358979)) +
1437 (IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
1438 evalcond[1] = x120;
1439 evalcond[2] = new_r20;
1440 evalcond[3] = new_r21;
1441 evalcond[4] = x121;
1442 evalcond[5] = x121;
1443 evalcond[6] = x122;
1444 evalcond[7] = new_r20;
1445 evalcond[8] = new_r21;
1446 evalcond[9] = x120;
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)
1459 {
1460 {
1461 IkReal j5array[1], cj5array[1], sj5array[1];
1462 bool j5valid[1] = { false };
1463 _nj5 = 1;
1464 if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
1465 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
1466 IKFAST_ATAN2_MAGTHRESH &&
1467 IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) <
1468 IKFAST_ATAN2_MAGTHRESH &&
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) <=
1472 IKFAST_SINCOS_THRESH)
1473 continue;
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)
1480 {
1481 j5array[0] -= IK2PI;
1482 }
1483 else if (j5array[0] < -IKPI)
1484 {
1485 j5array[0] += IK2PI;
1486 }
1487 j5valid[0] = true;
1488 for (int ij5 = 0; ij5 < 1; ++ij5)
1489 {
1490 if (!j5valid[ij5])
1491 {
1492 continue;
1493 }
1494 _ij5[0] = ij5;
1495 _ij5[1] = -1;
1496 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1497 {
1498 if (j5valid[iij5] &&
1499 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1500 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1501 {
1502 j5valid[iij5] = false;
1503 _ij5[1] = iij5;
1504 break;
1505 }
1506 }
1507 j5 = j5array[ij5];
1508 cj5 = cj5array[ij5];
1509 sj5 = sj5array[ij5];
1510 {
1511 IkReal evalcond[4];
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))));
1518 evalcond[1] =
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))));
1522 evalcond[3] =
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)
1527 {
1528 continue;
1529 }
1530 }
1531
1532 {
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);
1565 solutions.AddSolution(vinfos, vfree);
1566 }
1567 }
1568 }
1569 }
1570 else
1571 {
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))));
1576 evalcond[0] =
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;
1581 evalcond[4] = x128;
1582 evalcond[5] = x128;
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)
1601 {
1602 {
1603 IkReal j5array[1], cj5array[1], sj5array[1];
1604 bool j5valid[1] = { false };
1605 _nj5 = 1;
1606 if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) <
1607 IKFAST_ATAN2_MAGTHRESH &&
1608 IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1609 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
1610 IKFAST_ATAN2_MAGTHRESH &&
1611 IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
1612 IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
1613 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
1614 1) <= IKFAST_SINCOS_THRESH)
1615 continue;
1616 j5array[0] =
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)
1623 {
1624 j5array[0] -= IK2PI;
1625 }
1626 else if (j5array[0] < -IKPI)
1627 {
1628 j5array[0] += IK2PI;
1629 }
1630 j5valid[0] = true;
1631 for (int ij5 = 0; ij5 < 1; ++ij5)
1632 {
1633 if (!j5valid[ij5])
1634 {
1635 continue;
1636 }
1637 _ij5[0] = ij5;
1638 _ij5[1] = -1;
1639 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1640 {
1641 if (j5valid[iij5] &&
1642 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1643 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1644 {
1645 j5valid[iij5] = false;
1646 _ij5[1] = iij5;
1647 break;
1648 }
1649 }
1650 j5 = j5array[ij5];
1651 cj5 = cj5array[ij5];
1652 sj5 = sj5array[ij5];
1653 {
1654 IkReal evalcond[4];
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))));
1666 evalcond[3] =
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)
1670 {
1671 continue;
1672 }
1673 }
1674
1675 {
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);
1708 solutions.AddSolution(vinfos, vfree);
1709 }
1710 }
1711 }
1712 }
1713 else
1714 {
1715 if (1)
1716 {
1717 continue;
1718 }
1719 else
1720 {
1721 }
1722 }
1723 }
1724 }
1725 }
1726 else
1727 {
1728 {
1729 IkReal j5array[1], cj5array[1], sj5array[1];
1730 bool j5valid[1] = { false };
1731 _nj5 = 1;
1732 if (IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
1733 (((cj3) * (new_r10))))) < IKFAST_ATAN2_MAGTHRESH &&
1734 IKabs(((IkReal(-1.00000000000000)) * (new_r20) *
1735 (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
1736 IKFAST_ATAN2_MAGTHRESH &&
1737 IKabs(
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)))) -
1742 1) <= IKFAST_SINCOS_THRESH)
1743 continue;
1744 j5array[0] =
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)
1752 {
1753 j5array[0] -= IK2PI;
1754 }
1755 else if (j5array[0] < -IKPI)
1756 {
1757 j5array[0] += IK2PI;
1758 }
1759 j5valid[0] = true;
1760 for (int ij5 = 0; ij5 < 1; ++ij5)
1761 {
1762 if (!j5valid[ij5])
1763 {
1764 continue;
1765 }
1766 _ij5[0] = ij5;
1767 _ij5[1] = -1;
1768 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1769 {
1770 if (j5valid[iij5] &&
1771 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1772 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1773 {
1774 j5valid[iij5] = false;
1775 _ij5[1] = iij5;
1776 break;
1777 }
1778 }
1779 j5 = j5array[ij5];
1780 cj5 = cj5array[ij5];
1781 sj5 = sj5array[ij5];
1782 {
1783 IkReal evalcond[8];
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))));
1794 evalcond[1] =
1795 ((((IkReal(-1.00000000000000)) * (x132) * (x138))) + (new_r21));
1796 evalcond[2] =
1797 ((((IkReal(-1.00000000000000)) * (x140))) + (((cj3) * (new_r10))) +
1798 (((IkReal(-1.00000000000000)) * (new_r00) * (x134))));
1799 evalcond[3] =
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)
1814 {
1815 continue;
1816 }
1817 }
1818
1819 {
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);
1852 solutions.AddSolution(vinfos, vfree);
1853 }
1854 }
1855 }
1856 }
1857 }
1858 }
1859 else
1860 {
1861 {
1862 IkReal j5array[1], cj5array[1], sj5array[1];
1863 bool j5valid[1] = { false };
1864 _nj5 = 1;
1865 if (IKabs(((new_r21) *
1866 (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
1867 IKFAST_ATAN2_MAGTHRESH &&
1868 IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
1869 (((cj3) * (new_r11))))) < IKFAST_ATAN2_MAGTHRESH &&
1870 IKabs(IKsqr(((new_r21) *
1871 (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) +
1872 IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
1873 (((cj3) * (new_r11))))) -
1874 1) <= IKFAST_SINCOS_THRESH)
1875 continue;
1876 j5array[0] = IKatan2(
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)
1882 {
1883 j5array[0] -= IK2PI;
1884 }
1885 else if (j5array[0] < -IKPI)
1886 {
1887 j5array[0] += IK2PI;
1888 }
1889 j5valid[0] = true;
1890 for (int ij5 = 0; ij5 < 1; ++ij5)
1891 {
1892 if (!j5valid[ij5])
1893 {
1894 continue;
1895 }
1896 _ij5[0] = ij5;
1897 _ij5[1] = -1;
1898 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
1899 {
1900 if (j5valid[iij5] &&
1901 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
1902 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
1903 {
1904 j5valid[iij5] = false;
1905 _ij5[1] = iij5;
1906 break;
1907 }
1908 }
1909 j5 = j5array[ij5];
1910 cj5 = cj5array[ij5];
1911 sj5 = sj5array[ij5];
1912 {
1913 IkReal evalcond[8];
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))));
1925 evalcond[2] =
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)
1943 {
1944 continue;
1945 }
1946 }
1947
1948 {
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);
1981 solutions.AddSolution(vinfos, vfree);
1982 }
1983 }
1984 }
1985 }
1986 }
1987 }
1988 else
1989 {
1990 {
1991 IkReal j5array[1], cj5array[1], sj5array[1];
1992 bool j5valid[1] = { false };
1993 _nj5 = 1;
1994 if (IKabs(((gconst5) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH &&
1995 IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) <
1996 IKFAST_ATAN2_MAGTHRESH)
1997 continue;
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)
2003 {
2004 j5array[0] -= IK2PI;
2005 }
2006 else if (j5array[0] < -IKPI)
2007 {
2008 j5array[0] += IK2PI;
2009 }
2010 j5valid[0] = true;
2011 for (int ij5 = 0; ij5 < 1; ++ij5)
2012 {
2013 if (!j5valid[ij5])
2014 {
2015 continue;
2016 }
2017 _ij5[0] = ij5;
2018 _ij5[1] = -1;
2019 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2020 {
2021 if (j5valid[iij5] &&
2022 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2023 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2024 {
2025 j5valid[iij5] = false;
2026 _ij5[1] = iij5;
2027 break;
2028 }
2029 }
2030 j5 = j5array[ij5];
2031 cj5 = cj5array[ij5];
2032 sj5 = sj5array[ij5];
2033 {
2034 IkReal evalcond[8];
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)
2062 {
2063 continue;
2064 }
2065 }
2066
2067 {
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);
2100 solutions.AddSolution(vinfos, vfree);
2101 }
2102 }
2103 }
2104 }
2105 }
2106 }
2107 }
2108 }
2109 }
2110 }
2111 else
2112 {
2113 {
2114 IkReal j3array[1], cj3array[1], sj3array[1];
2115 bool j3valid[1] = { false };
2116 _nj3 = 1;
2117 IkReal x159 = ((gconst2) * (sj4));
2118 if (IKabs(((new_r12) * (x159))) < IKFAST_ATAN2_MAGTHRESH &&
2119 IKabs(((new_r02) * (x159))) < IKFAST_ATAN2_MAGTHRESH)
2120 continue;
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)
2125 {
2126 j3array[0] -= IK2PI;
2127 }
2128 else if (j3array[0] < -IKPI)
2129 {
2130 j3array[0] += IK2PI;
2131 }
2132 j3valid[0] = true;
2133 for (int ij3 = 0; ij3 < 1; ++ij3)
2134 {
2135 if (!j3valid[ij3])
2136 {
2137 continue;
2138 }
2139 _ij3[0] = ij3;
2140 _ij3[1] = -1;
2141 for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
2142 {
2143 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
2144 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
2145 {
2146 j3valid[iij3] = false;
2147 _ij3[1] = iij3;
2148 break;
2149 }
2150 }
2151 j3 = j3array[ij3];
2152 cj3 = cj3array[ij3];
2153 sj3 = sj3array[ij3];
2154 {
2155 IkReal evalcond[6];
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)
2174 {
2175 continue;
2176 }
2177 }
2178
2179 {
2180 IkReal dummyeval[1];
2181 IkReal gconst5;
2182 gconst5 = IKsign(sj4);
2183 dummyeval[0] = sj4;
2184 if (IKabs(dummyeval[0]) < 0.0000010000000000)
2185 {
2186 {
2187 IkReal dummyeval[1];
2188 dummyeval[0] = sj4;
2189 if (IKabs(dummyeval[0]) < 0.0000010000000000)
2190 {
2191 {
2192 IkReal dummyeval[1];
2193 dummyeval[0] = sj4;
2194 if (IKabs(dummyeval[0]) < 0.0000010000000000)
2195 {
2196 {
2197 IkReal evalcond[11];
2198 IkReal x167 = ((IkReal(-1.00000000000000)) + (new_r22));
2199 IkReal x168 =
2200 ((((cj3) * (new_r12))) + (((IkReal(-1.00000000000000)) * (new_r02) * (sj3))));
2201 IkReal x169 = ((((new_r12) * (sj3))) + (((cj3) * (new_r02))));
2202 evalcond[0] =
2203 ((IkReal(-3.14159265358979)) +
2204 (IKfmod(((IkReal(3.14159265358979)) + (j4)), IkReal(6.28318530717959))));
2205 evalcond[1] = x167;
2206 evalcond[2] = new_r20;
2207 evalcond[3] = new_r21;
2208 evalcond[4] = x168;
2209 evalcond[5] = x168;
2210 evalcond[6] = x169;
2211 evalcond[7] = new_r20;
2212 evalcond[8] = new_r21;
2213 evalcond[9] = x167;
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)
2226 {
2227 {
2228 IkReal j5array[1], cj5array[1], sj5array[1];
2229 bool j5valid[1] = { false };
2230 _nj5 = 1;
2231 if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) +
2232 (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) <
2233 IKFAST_ATAN2_MAGTHRESH &&
2234 IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) <
2235 IKFAST_ATAN2_MAGTHRESH &&
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) <=
2239 IKFAST_SINCOS_THRESH)
2240 continue;
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)
2247 {
2248 j5array[0] -= IK2PI;
2249 }
2250 else if (j5array[0] < -IKPI)
2251 {
2252 j5array[0] += IK2PI;
2253 }
2254 j5valid[0] = true;
2255 for (int ij5 = 0; ij5 < 1; ++ij5)
2256 {
2257 if (!j5valid[ij5])
2258 {
2259 continue;
2260 }
2261 _ij5[0] = ij5;
2262 _ij5[1] = -1;
2263 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2264 {
2265 if (j5valid[iij5] &&
2266 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2267 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2268 {
2269 j5valid[iij5] = false;
2270 _ij5[1] = iij5;
2271 break;
2272 }
2273 }
2274 j5 = j5array[ij5];
2275 cj5 = cj5array[ij5];
2276 sj5 = sj5array[ij5];
2277 {
2278 IkReal evalcond[4];
2279 IkReal x170 = IKsin(j5);
2280 IkReal x171 = ((IkReal(1.00000000000000)) * (sj3));
2281 IkReal x172 = ((IkReal(1.00000000000000)) * (IKcos(j5)));
2282 evalcond[0] =
2283 ((((IkReal(-1.00000000000000)) * (x170))) + (((cj3) * (new_r10))) +
2284 (((IkReal(-1.00000000000000)) * (new_r00) * (x171))));
2285 evalcond[1] =
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))));
2289 evalcond[3] =
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)
2294 {
2295 continue;
2296 }
2297 }
2298
2299 {
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);
2332 solutions.AddSolution(vinfos, vfree);
2333 }
2334 }
2335 }
2336 }
2337 else
2338 {
2339 IkReal x173 = ((new_r12) * (sj3));
2340 IkReal x174 = ((IkReal(1.00000000000000)) * (new_r02));
2341 IkReal x175 =
2342 ((((cj3) * (new_r12))) + (((IkReal(-1.00000000000000)) * (sj3) * (x174))));
2343 evalcond[0] =
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;
2348 evalcond[4] = x175;
2349 evalcond[5] = x175;
2350 evalcond[6] = ((x173) + (((cj3) * (new_r02))));
2351 evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20));
2352 evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21));
2353 evalcond[9] =
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)
2368 {
2369 {
2370 IkReal j5array[1], cj5array[1], sj5array[1];
2371 bool j5valid[1] = { false };
2372 _nj5 = 1;
2373 if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) <
2374 IKFAST_ATAN2_MAGTHRESH &&
2375 IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2376 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) <
2377 IKFAST_ATAN2_MAGTHRESH &&
2378 IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) +
2379 IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) +
2380 (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) -
2381 1) <= IKFAST_SINCOS_THRESH)
2382 continue;
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)
2389 {
2390 j5array[0] -= IK2PI;
2391 }
2392 else if (j5array[0] < -IKPI)
2393 {
2394 j5array[0] += IK2PI;
2395 }
2396 j5valid[0] = true;
2397 for (int ij5 = 0; ij5 < 1; ++ij5)
2398 {
2399 if (!j5valid[ij5])
2400 {
2401 continue;
2402 }
2403 _ij5[0] = ij5;
2404 _ij5[1] = -1;
2405 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2406 {
2407 if (j5valid[iij5] &&
2408 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2409 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2410 {
2411 j5valid[iij5] = false;
2412 _ij5[1] = iij5;
2413 break;
2414 }
2415 }
2416 j5 = j5array[ij5];
2417 cj5 = cj5array[ij5];
2418 sj5 = sj5array[ij5];
2419 {
2420 IkReal evalcond[4];
2421 IkReal x176 = IKcos(j5);
2422 IkReal x177 = ((IkReal(1.00000000000000)) * (sj3));
2423 IkReal x178 = ((IkReal(1.00000000000000)) * (IKsin(j5)));
2424 evalcond[0] =
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)
2435 {
2436 continue;
2437 }
2438 }
2439
2440 {
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);
2473 solutions.AddSolution(vinfos, vfree);
2474 }
2475 }
2476 }
2477 }
2478 else
2479 {
2480 if (1)
2481 {
2482 continue;
2483 }
2484 else
2485 {
2486 }
2487 }
2488 }
2489 }
2490 }
2491 else
2492 {
2493 {
2494 IkReal j5array[1], cj5array[1], sj5array[1];
2495 bool j5valid[1] = { false };
2496 _nj5 = 1;
2497 if (IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) +
2498 (((cj3) * (new_r10))))) < IKFAST_ATAN2_MAGTHRESH &&
2499 IKabs(((IkReal(-1.00000000000000)) * (new_r20) *
2500 (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
2501 IKFAST_ATAN2_MAGTHRESH &&
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)))) -
2506 1) <= IKFAST_SINCOS_THRESH)
2507 continue;
2508 j5array[0] = IKatan2(
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)
2515 {
2516 j5array[0] -= IK2PI;
2517 }
2518 else if (j5array[0] < -IKPI)
2519 {
2520 j5array[0] += IK2PI;
2521 }
2522 j5valid[0] = true;
2523 for (int ij5 = 0; ij5 < 1; ++ij5)
2524 {
2525 if (!j5valid[ij5])
2526 {
2527 continue;
2528 }
2529 _ij5[0] = ij5;
2530 _ij5[1] = -1;
2531 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2532 {
2533 if (j5valid[iij5] &&
2534 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2535 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2536 {
2537 j5valid[iij5] = false;
2538 _ij5[1] = iij5;
2539 break;
2540 }
2541 }
2542 j5 = j5array[ij5];
2543 cj5 = cj5array[ij5];
2544 sj5 = sj5array[ij5];
2545 {
2546 IkReal evalcond[8];
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))));
2558 evalcond[2] =
2559 ((((IkReal(-1.00000000000000)) * (new_r00) * (x181))) +
2560 (((IkReal(-1.00000000000000)) * (x187))) + (((cj3) * (new_r10))));
2561 evalcond[3] =
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)
2576 {
2577 continue;
2578 }
2579 }
2580
2581 {
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);
2614 solutions.AddSolution(vinfos, vfree);
2615 }
2616 }
2617 }
2618 }
2619 }
2620 }
2621 else
2622 {
2623 {
2624 IkReal j5array[1], cj5array[1], sj5array[1];
2625 bool j5valid[1] = { false };
2626 _nj5 = 1;
2627 if (IKabs(((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) <
2628 IKFAST_ATAN2_MAGTHRESH &&
2629 IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
2630 (((cj3) * (new_r11))))) < IKFAST_ATAN2_MAGTHRESH &&
2631 IKabs(IKsqr(((new_r21) *
2632 (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) : (IkReal)1.0e30)))) +
2633 IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) +
2634 (((cj3) * (new_r11))))) -
2635 1) <= IKFAST_SINCOS_THRESH)
2636 continue;
2637 j5array[0] = IKatan2(
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)
2643 {
2644 j5array[0] -= IK2PI;
2645 }
2646 else if (j5array[0] < -IKPI)
2647 {
2648 j5array[0] += IK2PI;
2649 }
2650 j5valid[0] = true;
2651 for (int ij5 = 0; ij5 < 1; ++ij5)
2652 {
2653 if (!j5valid[ij5])
2654 {
2655 continue;
2656 }
2657 _ij5[0] = ij5;
2658 _ij5[1] = -1;
2659 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2660 {
2661 if (j5valid[iij5] &&
2662 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2663 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2664 {
2665 j5valid[iij5] = false;
2666 _ij5[1] = iij5;
2667 break;
2668 }
2669 }
2670 j5 = j5array[ij5];
2671 cj5 = cj5array[ij5];
2672 sj5 = sj5array[ij5];
2673 {
2674 IkReal evalcond[8];
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))));
2686 evalcond[2] =
2687 ((((IkReal(-1.00000000000000)) * (x196))) +
2688 (((IkReal(-1.00000000000000)) * (new_r00) * (x190))) + (((cj3) * (new_r10))));
2689 evalcond[3] =
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))));
2697 evalcond[7] =
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)
2704 {
2705 continue;
2706 }
2707 }
2708
2709 {
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);
2742 solutions.AddSolution(vinfos, vfree);
2743 }
2744 }
2745 }
2746 }
2747 }
2748 }
2749 else
2750 {
2751 {
2752 IkReal j5array[1], cj5array[1], sj5array[1];
2753 bool j5valid[1] = { false };
2754 _nj5 = 1;
2755 if (IKabs(((gconst5) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH &&
2756 IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH)
2757 continue;
2758 j5array[0] =
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)
2763 {
2764 j5array[0] -= IK2PI;
2765 }
2766 else if (j5array[0] < -IKPI)
2767 {
2768 j5array[0] += IK2PI;
2769 }
2770 j5valid[0] = true;
2771 for (int ij5 = 0; ij5 < 1; ++ij5)
2772 {
2773 if (!j5valid[ij5])
2774 {
2775 continue;
2776 }
2777 _ij5[0] = ij5;
2778 _ij5[1] = -1;
2779 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2780 {
2781 if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2782 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2783 {
2784 j5valid[iij5] = false;
2785 _ij5[1] = iij5;
2786 break;
2787 }
2788 }
2789 j5 = j5array[ij5];
2790 cj5 = cj5array[ij5];
2791 sj5 = sj5array[ij5];
2792 {
2793 IkReal evalcond[8];
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))));
2810 evalcond[5] =
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)
2821 {
2822 continue;
2823 }
2824 }
2825
2826 {
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);
2859 solutions.AddSolution(vinfos, vfree);
2860 }
2861 }
2862 }
2863 }
2864 }
2865 }
2866 }
2867 }
2868 }
2869 }
2870 else
2871 {
2872 {
2873 IkReal j5array[1], cj5array[1], sj5array[1];
2874 bool j5valid[1] = { false };
2875 _nj5 = 1;
2876 if (IKabs(((gconst4) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH &&
2877 IKabs(((IkReal(-1.00000000000000)) * (gconst4) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH)
2878 continue;
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)
2883 {
2884 j5array[0] -= IK2PI;
2885 }
2886 else if (j5array[0] < -IKPI)
2887 {
2888 j5array[0] += IK2PI;
2889 }
2890 j5valid[0] = true;
2891 for (int ij5 = 0; ij5 < 1; ++ij5)
2892 {
2893 if (!j5valid[ij5])
2894 {
2895 continue;
2896 }
2897 _ij5[0] = ij5;
2898 _ij5[1] = -1;
2899 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2900 {
2901 if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2902 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2903 {
2904 j5valid[iij5] = false;
2905 _ij5[1] = iij5;
2906 break;
2907 }
2908 }
2909 j5 = j5array[ij5];
2910 cj5 = cj5array[ij5];
2911 sj5 = sj5array[ij5];
2912 {
2913 IkReal evalcond[2];
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)
2917 {
2918 continue;
2919 }
2920 }
2921
2922 {
2923 IkReal dummyeval[1];
2924 IkReal gconst6;
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)
2928 {
2929 {
2930 IkReal dummyeval[1];
2931 IkReal gconst7;
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)
2937 {
2938 continue;
2939 }
2940 else
2941 {
2942 {
2943 IkReal j3array[1], cj3array[1], sj3array[1];
2944 bool j3valid[1] = { false };
2945 _nj3 = 1;
2946 IkReal x206 = ((cj4) * (gconst7) * (sj5));
2947 if (IKabs(((new_r12) * (x206))) < IKFAST_ATAN2_MAGTHRESH &&
2948 IKabs(((new_r02) * (x206))) < IKFAST_ATAN2_MAGTHRESH)
2949 continue;
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)
2954 {
2955 j3array[0] -= IK2PI;
2956 }
2957 else if (j3array[0] < -IKPI)
2958 {
2959 j3array[0] += IK2PI;
2960 }
2961 j3valid[0] = true;
2962 for (int ij3 = 0; ij3 < 1; ++ij3)
2963 {
2964 if (!j3valid[ij3])
2965 {
2966 continue;
2967 }
2968 _ij3[0] = ij3;
2969 _ij3[1] = -1;
2970 for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
2971 {
2972 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
2973 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
2974 {
2975 j3valid[iij3] = false;
2976 _ij3[1] = iij3;
2977 break;
2978 }
2979 }
2980 j3 = j3array[ij3];
2981 cj3 = cj3array[ij3];
2982 sj3 = sj3array[ij3];
2983 {
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));
2995 evalcond[0] =
2996 ((((IkReal(-1.00000000000000)) * (new_r02) * (x216))) + (((new_r12) * (x208))));
2997 evalcond[1] = ((((IkReal(-1.00000000000000)) * (x210))) + (((new_r12) * (x207))) +
2998 (((new_r02) * (x208))));
2999 evalcond[2] =
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))));
3013 evalcond[10] =
3014 ((((new_r11) * (x213))) + (((IkReal(-1.00000000000000)) * (new_r21) * (x210))) +
3015 (sj5) + (((new_r01) * (x211))));
3016 evalcond[11] =
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)
3025 {
3026 continue;
3027 }
3028 }
3029
3030 {
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);
3063 solutions.AddSolution(vinfos, vfree);
3064 }
3065 }
3066 }
3067 }
3068 }
3069 }
3070 else
3071 {
3072 {
3073 IkReal j3array[1], cj3array[1], sj3array[1];
3074 bool j3valid[1] = { false };
3075 _nj3 = 1;
3076 IkReal x217 = ((gconst6) * (sj4));
3077 if (IKabs(((new_r12) * (x217))) < IKFAST_ATAN2_MAGTHRESH &&
3078 IKabs(((new_r02) * (x217))) < IKFAST_ATAN2_MAGTHRESH)
3079 continue;
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)
3084 {
3085 j3array[0] -= IK2PI;
3086 }
3087 else if (j3array[0] < -IKPI)
3088 {
3089 j3array[0] += IK2PI;
3090 }
3091 j3valid[0] = true;
3092 for (int ij3 = 0; ij3 < 1; ++ij3)
3093 {
3094 if (!j3valid[ij3])
3095 {
3096 continue;
3097 }
3098 _ij3[0] = ij3;
3099 _ij3[1] = -1;
3100 for (int iij3 = ij3 + 1; iij3 < 1; ++iij3)
3101 {
3102 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
3103 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
3104 {
3105 j3valid[iij3] = false;
3106 _ij3[1] = iij3;
3107 break;
3108 }
3109 }
3110 j3 = j3array[ij3];
3111 cj3 = cj3array[ij3];
3112 sj3 = sj3array[ij3];
3113 {
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));
3125 evalcond[0] =
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))));
3144 evalcond[11] =
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)
3153 {
3154 continue;
3155 }
3156 }
3157
3158 {
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);
3191 solutions.AddSolution(vinfos, vfree);
3192 }
3193 }
3194 }
3195 }
3196 }
3197 }
3198 }
3199 }
3200 }
3201 }
3202 }
3203 }
3204 }
3205};
3206
3209IKFAST_API bool
3210ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
3211{
3212 IKSolver solver;
3213 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
3214}
3215
3216IKFAST_API const char* GetKinematicsHash()
3217{
3218 return "<robot:genericrobot - abb_irb2400 (1f04c8a90b29778d31a8f2cb88b4a166)>";
3219}
3220
3221IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
3222
3223#ifdef IKFAST_NAMESPACE
3224} // end namespace
3225#endif
3226
3227#ifndef IKFAST_NO_MAIN
3228#include <stdio.h>
3229#include <stdlib.h>
3230#ifdef IKFAST_NAMESPACE
3231using namespace IKFAST_NAMESPACE;
3232#endif
3233int main(int argc, char** argv)
3234{
3235 if (argc != 12 + GetNumFreeParameters() + 1)
3236 {
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",
3241 GetNumFreeParameters());
3242 return 1;
3243 }
3244
3245 IkSolutionList<IkReal> solutions;
3246 std::vector<IkReal> vfree(GetNumFreeParameters());
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);
3263
3264 if (!bSuccess)
3265 {
3266 fprintf(stderr, "Failed to get ik solution\n");
3267 return -1;
3268 }
3269
3270 printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
3271 std::vector<IkReal> solvalues(GetNumJoints());
3272 for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
3273 {
3274 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
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]);
3280 printf("\n");
3281 }
3282
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 };
3285 ComputeFk(sol, trans, rot);
3286 printf("FK: %f %f %f\n", trans[0], trans[1], trans[2]);
3287
3288 return 0;
3289}
3290
3291#endif
3292TESSERACT_COMMON_IGNORE_WARNINGS_POP
3293// clang-format on
IKPI
#define IKPI
Definition: abb_irb2400_ikfast_solver.hpp:79
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: abb_irb2400_ikfast_solver.hpp:142
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: abb_irb2400_ikfast_solver.hpp:32
IKasin
float IKasin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:150
dgetrs_
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)
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:380
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: abb_irb2400_ikfast_solver.hpp:381
IKPI_2
#define IKPI_2
Definition: abb_irb2400_ikfast_solver.hpp:80
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: abb_irb2400_ikfast_solver.hpp:3221
IKatan2
float IKatan2(float fy, float fx)
Definition: abb_irb2400_ikfast_solver.hpp:229
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:137
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: abb_irb2400_ikfast_solver.hpp:383
IKacos
float IKacos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:191
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: abb_irb2400_ikfast_solver.hpp:3216
IKsqrt
float IKsqrt(float f)
Definition: abb_irb2400_ikfast_solver.hpp:217
IKsin
float IKsin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:211
IKsign
float IKsign(float f)
Definition: abb_irb2400_ikfast_solver.hpp:256
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKfmod
float IKfmod(float x, float y)
Definition: abb_irb2400_ikfast_solver.hpp:172
IKcos
float IKcos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:213
IKsqr
float IKsqr(float f)
Definition: abb_irb2400_ikfast_solver.hpp:129
GetIkType
IKFAST_API int GetIkType()
Definition: abb_irb2400_ikfast_solver.hpp:385
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: abb_irb2400_ikfast_solver.hpp:284
IKtan
float IKtan(float f)
Definition: abb_irb2400_ikfast_solver.hpp:215
IK2PI
#define IK2PI
Definition: abb_irb2400_ikfast_solver.hpp:78
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:3210
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:147
IKlog
float IKlog(float f)
Definition: abb_irb2400_ikfast_solver.hpp:132
dgeev_
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)
IKFAST_STRINGIZE
#define IKFAST_STRINGIZE(s)
Definition: abb_irb2400_ikfast_solver.hpp:41
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:379
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: abb_irb2400_ikfast_solver.hpp:59
IKabs
float IKabs(float f)
Definition: abb_irb2400_ikfast_solver.hpp:126
IKSolver
Definition: abb_irb2400_ikfast_solver.hpp:388
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:867
IKSolver::new_px
IkReal new_px
Definition: abb_irb2400_ikfast_solver.hpp:392
IKSolver::new_py
IkReal new_py
Definition: abb_irb2400_ikfast_solver.hpp:393
IKSolver::cj0
IkReal cj0
Definition: abb_irb2400_ikfast_solver.hpp:390
IKSolver::htj5
IkReal htj5
Definition: abb_irb2400_ikfast_solver.hpp:391
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:396
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:72
ikfast::IkSolutionBase::GetFree
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...
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:103
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
ikfast::IkSolutionListBase::AddSolution
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::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:259
ikfast::IkSolutionList::GetSolution
const IkSolutionBase< T > & GetSolution(size_t index) const override
returns the solution pointer
Definition: ikfast.h:268
ikfast::IkSolutionList::GetNumSolutions
size_t GetNumSolutions() const override
returns the number of solutions stored
Definition: ikfast.h:279
main
int main(int argc, char **argv)
Definition: create_convex_hull.cpp:43
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:45
m
tesseract_kinematics::Manipulability m
Definition: kinematics_core_unit.cpp:179
macros.h
Common Tesseract Macros.
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
TESSERACT_COMMON_IGNORE_WARNINGS_POP
Definition: create_convex_hull.cpp:36
ikfast
Definition: ikfast.h:48
j
JointDynamics j
Definition: tesseract_scene_graph_joint_unit.cpp:15
info
KinematicsInformation info
Definition: tesseract_srdf_unit.cpp:1765
Generated on Thu Mar 30 2023 23:19:52 for Tesseract by doxygen 1.9.5