1 /// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE
2 /// \author Rosen Diankov
4 /// Licensed under the Apache License, Version 2.0 (the "License");
5 /// you may not use this file except in compliance with the License.
6 /// You may obtain a copy of the License at
7 /// http://www.apache.org/licenses/LICENSE-2.0
9 /// Unless required by applicable law or agreed to in writing, software
10 /// distributed under the License is distributed on an "AS IS" BASIS,
11 /// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 /// See the License for the specific language governing permissions and
13 /// limitations under the License.
15 /// ikfast version 0x10000049 generated on 2020-01-01 11:08:21.983960
16 /// To compile with gcc:
17 /// gcc -lstdc++ ik.cpp
18 /// To compile without any main function as a shared object (might need -llapack):
19 /// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp
20 #define IKFAST_HAS_LIBRARY
21 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
22 using namespace ikfast;
24 // check if the included ikfast version matches what this file was compiled with
25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
26 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049);
40 #ifndef __PRETTY_FUNCTION__
41 #define __PRETTY_FUNCTION__ __FUNCDNAME__
45 #ifndef __PRETTY_FUNCTION__
46 #define __PRETTY_FUNCTION__ __func__
49 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
54 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
56 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
59 #define IK2PI ((IkReal)6.28318530717959)
60 #define IKPI ((IkReal)3.14159265358979)
61 #define IKPI_2 ((IkReal)1.57079632679490)
71 //#define isfinite _isfinite
77 void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
78 void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
79 void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
80 void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
81 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);
82 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);
85 using namespace std; // necessary to get std math routines
87 #ifdef IKFAST_NAMESPACE
88 namespace IKFAST_NAMESPACE {
91 inline float IKabs(float f) { return fabsf(f); }
92 inline double IKabs(double f) { return fabs(f); }
94 inline float IKsqr(float f) { return f*f; }
95 inline double IKsqr(double f) { return f*f; }
97 inline float IKlog(float f) { return logf(f); }
98 inline double IKlog(double f) { return log(f); }
100 // allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
101 #ifndef IKFAST_SINCOS_THRESH
102 #define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
105 // used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
106 #ifndef IKFAST_ATAN2_MAGTHRESH
107 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
110 // minimum distance of separate solutions
111 #ifndef IKFAST_SOLUTION_THRESH
112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
115 // there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
116 #ifndef IKFAST_EVALCOND_THRESH
117 #define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
121 inline float IKasin(float f)
123 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
124 if( f <= -1 ) return float(-IKPI_2);
125 else if( f >= 1 ) return float(IKPI_2);
128 inline double IKasin(double f)
130 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
131 if( f <= -1 ) return -IKPI_2;
132 else if( f >= 1 ) return IKPI_2;
136 // return positive value in [0,y)
137 inline float IKfmod(float x, float y)
145 // return positive value in [0,y)
146 inline double IKfmod(double x, double y)
154 inline float IKacos(float f)
156 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
157 if( f <= -1 ) return float(IKPI);
158 else if( f >= 1 ) return float(0);
161 inline double IKacos(double f)
163 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
164 if( f <= -1 ) return IKPI;
165 else if( f >= 1 ) return 0;
168 inline float IKsin(float f) { return sinf(f); }
169 inline double IKsin(double f) { return sin(f); }
170 inline float IKcos(float f) { return cosf(f); }
171 inline double IKcos(double f) { return cos(f); }
172 inline float IKtan(float f) { return tanf(f); }
173 inline double IKtan(double f) { return tan(f); }
174 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
175 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
176 inline float IKatan2Simple(float fy, float fx) {
177 return atan2f(fy,fx);
179 inline float IKatan2(float fy, float fx) {
181 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
182 return float(IKPI_2);
184 else if( isnan(fx) ) {
187 return atan2f(fy,fx);
189 inline double IKatan2Simple(double fy, double fx) {
192 inline double IKatan2(double fy, double fx) {
194 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
197 else if( isnan(fx) ) {
203 template <typename T>
210 template <typename T>
211 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
216 if( !isnan(fy) && !isnan(fx) ) {
217 if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
218 ret.value = IKatan2Simple(fy,fx);
225 inline float IKsign(float f) {
235 inline double IKsign(double f) {
245 template <typename T>
246 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
264 ret.value = (T)1.0e30;
268 ret.value = T(1.0)/f;
273 int num = n > 0 ? n : -n;
277 else if( num == 3 ) {
292 ret.value = T(1.0)/ret.value;
297 /// solves the forward kinematics equations.
298 /// \param pfree is an array specifying the free joints of the chain.
299 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
300 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
301 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19;
322 IkReal x20=((1.0)*x17);
323 eetrans[0]=((0.012)+((x0*x11))+((x16*x9))+((x7*(((((-1.0)*x20*x8))+((x13*x16))))))+(((0.024)*x16))+((x5*(((((-1.0)*x16*x8))+(((-1.0)*x13*x20))))))+(((-1.0)*x12*x20)));
324 IkReal x21=((1.0)*x8);
325 IkReal x22=((1.0)*x19);
326 eetrans[1]=(((x5*(((((-1.0)*x14*x21))+(((-1.0)*x13*x22))))))+((x14*x9))+((x11*x6))+((x7*(((((-1.0)*x19*x21))+((x13*x14))))))+(((0.024)*x14))+(((-1.0)*x12*x22)));
327 IkReal x23=((1.0)*x2);
328 IkReal x24=((1.0)*x3);
329 eetrans[2]=((0.0765)+(((0.128)*x2))+((x7*(((((-1.0)*x23*x8))+(((-1.0)*x13*x24))))))+(((-1.0)*x24*x9))+((x5*((((x3*x8))+(((-1.0)*x13*x23))))))+(((-0.024)*x3))+(((-1.0)*x12*x23)));
330 IkReal x25=((1.0)*x10);
331 if( ((((x5*(((((-1.0)*x18))+(((-1.0)*x2*x25))))))+((x7*(((((1.0)*x15))+(((-1.0)*x25*x3)))))))) < -1-IKFAST_SINCOS_THRESH || ((((x5*(((((-1.0)*x18))+(((-1.0)*x2*x25))))))+((x7*(((((1.0)*x15))+(((-1.0)*x25*x3)))))))) > 1+IKFAST_SINCOS_THRESH )
333 eerot[0]=IKacos((((x5*(((((-1.0)*x18))+(((-1.0)*x2*x25))))))+((x7*(((((1.0)*x15))+(((-1.0)*x25*x3))))))));
339 IKFAST_API int GetNumFreeParameters() { return 0; }
340 IKFAST_API int* GetFreeParameters() { return NULL; }
341 IKFAST_API int GetNumJoints() { return 4; }
343 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
345 IKFAST_API int GetIkType() { return 0x4400000d; }
349 IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
350 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3;
352 IkReal j100, cj100, sj100;
353 unsigned char _ij100[2], _nj100;
354 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
355 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1;
356 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
358 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
361 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
366 r00 = new_r00; px = new_px; py = new_py; pz = new_pz;
368 pp=((px*px)+(py*py)+(pz*pz));
370 IkReal j0array[2], cj0array[2], sj0array[2];
371 bool j0valid[2]={false};
373 CheckValue<IkReal> x27 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(((-0.012)+px)),IKFAST_ATAN2_MAGTHRESH);
377 IkReal x26=x27.value;
378 j0array[0]=((-1.0)*x26);
379 sj0array[0]=IKsin(j0array[0]);
380 cj0array[0]=IKcos(j0array[0]);
381 j0array[1]=((3.14159265358979)+(((-1.0)*x26)));
382 sj0array[1]=IKsin(j0array[1]);
383 cj0array[1]=IKcos(j0array[1]);
384 if( j0array[0] > IKPI )
388 else if( j0array[0] < -IKPI )
392 if( j0array[1] > IKPI )
396 else if( j0array[1] < -IKPI )
400 for(int ij0 = 0; ij0 < 2; ++ij0)
406 _ij0[0] = ij0; _ij0[1] = -1;
407 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
409 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
411 j0valid[iij0]=false; _ij0[1] = iij0; break;
414 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
416 IkReal op[162], zeror[48];
418 IkReal x28=IKcos(j0);
419 IkReal x29=((0.409)*pz);
420 IkReal x30=((0.024)*px);
421 IkReal x31=IKsin(j0);
422 IkReal x32=((0.101432)+(((-0.496)*pz)));
423 IkReal x33=((-0.2045)+pz);
424 IkReal x34=((2.0)*pz);
425 IkReal x35=((0.153)+(((-1.0)*x34)));
426 IkReal x36=((0.103)*pz);
427 IkReal x37=((0.496)*pz);
428 IkReal x38=((-0.103)+(((-1.0)*x34)));
429 IkReal x39=((-0.0515)+(((-1.0)*pz)));
430 IkReal x40=((1.0)*(px*px));
431 IkReal x41=((1.0)*(py*py));
432 IkReal x42=((1.0)*(pz*pz));
433 IkReal x43=((1.0)*(IKcos(r00)));
434 IkReal x44=((-1.0)+(((-1.0)*x43)));
435 IkReal x45=((1.0)+(((-1.0)*x43)));
436 IkReal x46=((0.012)*x28);
437 IkReal x47=((-0.409)+x34);
440 IkReal x50=((0.006144)*x28);
441 IkReal x51=((-0.153)+x34);
442 IkReal x52=((0.000576)*x28);
443 IkReal x53=((0.003552)*x28);
444 IkReal x54=((0.025544)+x37);
445 IkReal x55=((0.0024)*x28);
446 IkReal x56=((0.048)*x48);
447 IkReal x57=((0.048)*x49);
448 IkReal x58=((0.512)*x49);
449 IkReal x59=((0.512)*x48);
450 IkReal x60=((0.296)*x48);
451 IkReal x61=((0.296)*x49);
452 IkReal x62=((0.2)*x48);
453 IkReal x63=((0.2)*x49);
454 IkReal x64=(x30+x29);
455 IkReal x65=(x30+x52);
456 IkReal x66=(x48+x49);
457 IkReal x67=((((1.0)*x48))+(((1.0)*x49)));
458 IkReal x68=(x59+x58);
459 IkReal x69=(x62+x63);
460 IkReal x70=(x57+x56);
461 IkReal x71=(x60+x61);
462 IkReal x72=((0.274)+(((-1.0)*x46))+x66);
463 IkReal x73=((0.022)+(((-1.0)*x46))+x66);
464 IkReal x74=(x42+x40+x41);
465 IkReal x75=((((-0.992)*x49))+(((-0.992)*x48))+(((0.011904)*x28)));
466 IkReal x76=((((4.0)*x48))+(((4.0)*x49))+(((-0.048)*x28)));
467 IkReal x77=((((-0.024)*x28))+(((2.0)*x49))+(((2.0)*x48)));
468 IkReal x78=((0.274)+x46+(((-1.0)*x67)));
469 IkReal x79=((0.022)+x46+(((-1.0)*x67)));
470 IkReal x80=(x74+x36);
471 IkReal x81=(x74+x52);
472 IkReal x82=((0.007344)+(((-0.096)*pz))+x68+(((-1.0)*x50)));
473 IkReal x83=((0.045288)+(((-0.592)*pz))+x68+(((-1.0)*x50)));
474 IkReal x84=((-0.0306)+(((0.4)*pz))+x68+(((-1.0)*x50)));
475 IkReal x85=(x70+x80);
476 IkReal x86=((0.01995975)+x70+x64+(((-1.0)*x81)));
477 IkReal x87=((-0.04253625)+x70+x64+(((-1.0)*x81)));
478 IkReal x88=((-0.04799225)+(((-1.0)*x74))+x71+x64+(((-1.0)*x53)));
479 IkReal x89=((-0.03608825)+(((-1.0)*x74))+x55+x64+(((-1.0)*x69)));
480 IkReal x90=((0.05912775)+x65+(((-1.0)*x85)));
481 IkReal x91=((-0.00336825)+x65+(((-1.0)*x85)));
482 IkReal x92=((-0.00882425)+(((-1.0)*x71))+x30+x53+(((-1.0)*x80)));
483 IkReal x93=((0.00307975)+x30+x69+(((-1.0)*x80))+(((-1.0)*x55)));
517 op[33]=((-0.226)+x46+(((-1.0)*x67)));
519 op[35]=((0.026)+x46+(((-1.0)*x67)));
526 op[42]=((0.226)+(((-1.0)*x46))+x66);
528 op[44]=((-0.026)+(((-1.0)*x46))+x66);
625 op[141]=((-0.226)+(((-1.0)*x46))+x66);
627 op[143]=((0.026)+(((-1.0)*x46))+x66);
634 op[150]=((0.226)+x46+(((-1.0)*x67)));
636 op[152]=((-0.026)+x46+(((-1.0)*x67)));
646 solvedialyticpoly12qep(op,zeror,numroots);
647 IkReal j1array[16], cj1array[16], sj1array[16], j2array[16], cj2array[16], sj2array[16], j3array[16], cj3array[16], sj3array[16];
648 int numsolutions = 0;
649 for(int ij1 = 0; ij1 < numroots; ij1 += 3)
651 IkReal htj1 = zeror[ij1+0], htj2 = zeror[ij1+1], htj3 = zeror[ij1+2];
652 if(isnan(htj1)||isnan(htj2)||isnan(htj3)){
655 j1array[numsolutions]=((2.0)*(atan(htj1)));
656 j2array[numsolutions]=((2.0)*(atan(htj2)));
657 j3array[numsolutions]=((2.0)*(atan(htj3)));
659 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
660 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
663 IkReal x94=htj1*htj1;
664 CheckValue<IkReal> x95=IKPowWithIntegerCheck(((1.0)+x94),-1);
668 cj1array[numsolutions]=((x95.value)*(((1.0)+(((-1.0)*x94)))));
669 CheckValue<IkReal> x96=IKPowWithIntegerCheck(((1.0)+(htj1*htj1)),-1);
673 sj1array[numsolutions]=((2.0)*htj1*(x96.value));
676 cj2array[numsolutions] = IKcos(j2array[numsolutions]);
677 sj2array[numsolutions] = IKsin(j2array[numsolutions]);
680 IkReal x97=htj2*htj2;
681 CheckValue<IkReal> x98=IKPowWithIntegerCheck(((1.0)+x97),-1);
685 cj2array[numsolutions]=((x98.value)*(((1.0)+(((-1.0)*x97)))));
686 CheckValue<IkReal> x99=IKPowWithIntegerCheck(((1.0)+(htj2*htj2)),-1);
690 sj2array[numsolutions]=((2.0)*htj2*(x99.value));
693 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
694 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
697 IkReal x100=htj3*htj3;
698 CheckValue<IkReal> x101=IKPowWithIntegerCheck(((1.0)+x100),-1);
702 cj3array[numsolutions]=((x101.value)*(((1.0)+(((-1.0)*x100)))));
703 CheckValue<IkReal> x102=IKPowWithIntegerCheck(((1.0)+(htj3*htj3)),-1);
707 sj3array[numsolutions]=((2.0)*htj3*(x102.value));
709 if( j1array[numsolutions] > IKPI )
711 j1array[numsolutions]-=IK2PI;
713 else if( j1array[numsolutions] < -IKPI )
715 j1array[numsolutions]+=IK2PI;
717 if( j2array[numsolutions] > IKPI )
719 j2array[numsolutions]-=IK2PI;
721 else if( j2array[numsolutions] < -IKPI )
723 j2array[numsolutions]+=IK2PI;
725 if( j3array[numsolutions] > IKPI )
727 j3array[numsolutions]-=IK2PI;
729 else if( j3array[numsolutions] < -IKPI )
731 j3array[numsolutions]+=IK2PI;
735 bool j1valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
739 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
745 _ij1[0] = ij1; _ij1[1] = -1;
746 _ij2[0] = 0; _ij2[1] = -1;
747 _ij3[0] = 0; _ij3[1] = -1;
748 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
750 if( !j1valid[iij1] ) { continue; }
751 if( IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(cj2array[ij1]-cj2array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij1]-sj2array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(cj3array[ij1]-cj3array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij1]-sj3array[iij1]) < IKFAST_SOLUTION_THRESH && 1 )
753 j1valid[iij1]=false; _ij1[1] = iij1; _ij2[1] = 0; _ij3[1] = 0; break;
756 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
758 j2 = j2array[ij1]; cj2 = cj2array[ij1]; sj2 = sj2array[ij1];
760 j3 = j3array[ij1]; cj3 = cj3array[ij1]; sj3 = sj3array[ij1];
763 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(4);
764 vinfos[0].jointtype = 1;
765 vinfos[0].foffset = j0;
766 vinfos[0].indices[0] = _ij0[0];
767 vinfos[0].indices[1] = _ij0[1];
768 vinfos[0].maxsolutions = _nj0;
769 vinfos[1].jointtype = 1;
770 vinfos[1].foffset = j1;
771 vinfos[1].indices[0] = _ij1[0];
772 vinfos[1].indices[1] = _ij1[1];
773 vinfos[1].maxsolutions = _nj1;
774 vinfos[2].jointtype = 1;
775 vinfos[2].foffset = j2;
776 vinfos[2].indices[0] = _ij2[0];
777 vinfos[2].indices[1] = _ij2[1];
778 vinfos[2].maxsolutions = _nj2;
779 vinfos[3].jointtype = 1;
780 vinfos[3].foffset = j3;
781 vinfos[3].indices[0] = _ij3[0];
782 vinfos[3].indices[1] = _ij3[1];
783 vinfos[3].maxsolutions = _nj3;
784 std::vector<int> vfree(0);
785 solutions.AddSolution(vinfos,vfree);
791 return solutions.GetNumSolutions()>0;
794 /// \brief Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994)
796 /// matcoeffs is of length 54*3, for 3 matrices
797 static inline void solvedialyticpoly12qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
799 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
800 IkReal IKFAST_ALIGNED16(M[24*24]) = {0};
801 IkReal IKFAST_ALIGNED16(A[12*12]);
802 IkReal IKFAST_ALIGNED16(work[24*24*23]);
804 int info, coeffindex;
805 const int worksize=24*24*23;
806 const int matrixdim = 12;
807 const int matrixdim2 = 24;
809 // first setup M = [0 I; -C -B] and A
811 for(int j = 0; j < 6; ++j) {
812 for(int k = 0; k < 9; ++k) {
813 M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -matcoeffs[coeffindex++];
816 for(int j = 0; j < 6; ++j) {
817 for(int k = 0; k < 9; ++k) {
818 M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
821 for(int j = 0; j < 6; ++j) {
822 for(int k = 0; k < 9; ++k) {
823 A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = matcoeffs[coeffindex++];
825 for(int k = 0; k < 3; ++k) {
826 A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
829 const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
831 bool bsingular = true;
833 dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
836 for(int j = 0; j < matrixdim; ++j) {
837 if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
849 // transform by the linear functional
851 const IkReal* lf = lfpossibilities[lfindex];
852 // have to reinitialize A
854 for(int j = 0; j < 6; ++j) {
855 for(int k = 0; k < 9; ++k) {
856 IkReal a = matcoeffs[coeffindex+108], b = matcoeffs[coeffindex+54], c = matcoeffs[coeffindex];
857 A[(j+6)+matrixdim*k] = A[j+matrixdim*(k+3)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
858 M[matrixdim+(j+6)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+3)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
859 M[matrixdim+(j+6)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+3)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
862 for(int k = 0; k < 3; ++k) {
863 A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
871 dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
876 // set identity in upper corner
877 for(int j = 0; j < matrixdim; ++j) {
878 M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
880 IkReal IKFAST_ALIGNED16(wr[24]);
881 IkReal IKFAST_ALIGNED16(wi[24]);
882 IkReal IKFAST_ALIGNED16(vr[24*24]);
884 dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
888 IkReal Breal[matrixdim-1];
889 for(int i = 0; i < matrixdim2; ++i) {
890 if( IKabs(wi[i]) < tol*100 ) {
891 IkReal* ev = vr+matrixdim2*i;
892 if( IKabs(wr[i]) > 1 ) {
895 // consistency has to be checked!!
896 if( IKabs(ev[0]) < tol ) {
899 IkReal iconst = 1/ev[0];
900 for(int j = 1; j < matrixdim; ++j) {
901 Breal[j-1] = ev[j]*iconst;
903 if( checkconsistency12(Breal) ) {
905 const IkReal* lf = lfpossibilities[lfindex];
906 rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
909 rawroots[numroots++] = wr[i];
911 bool bsmall0=IKabs(ev[0]) > IKabs(ev[3]);
912 bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
913 if( bsmall0 && bsmall1 ) {
914 rawroots[numroots++] = ev[3]/ev[0];
915 rawroots[numroots++] = ev[1]/ev[0];
917 else if( bsmall0 && !bsmall1 ) {
918 rawroots[numroots++] = ev[5]/ev[2];
919 rawroots[numroots++] = ev[2]/ev[1];
921 else if( !bsmall0 && bsmall1 ) {
922 rawroots[numroots++] = ev[9]/ev[6];
923 rawroots[numroots++] = ev[10]/ev[9];
925 else if( !bsmall0 && !bsmall1 ) {
926 rawroots[numroots++] = ev[11]/ev[8];
927 rawroots[numroots++] = ev[11]/ev[10];
933 static inline bool checkconsistency12(const IkReal* Breal)
936 for(int i = 0; i < 11; ++i) {
937 norm += IKabs(Breal[i]);
939 IkReal tol = 1e-6*norm; // have to increase the threshold since many computations are involved
940 return IKabs(Breal[0]*Breal[0]-Breal[1]) < tol && IKabs(Breal[0]*Breal[2]-Breal[3]) < tol && IKabs(Breal[1]*Breal[2]-Breal[4]) < tol && IKabs(Breal[2]*Breal[2]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol && IKabs(Breal[1]*Breal[5]-Breal[7]) < tol && IKabs(Breal[2]*Breal[5]-Breal[8]) < tol && IKabs(Breal[0]*Breal[8]-Breal[9]) < tol && IKabs(Breal[1]*Breal[8]-Breal[10]) < tol;
945 /// solves the inverse kinematics equations.
946 /// \param pfree is an array specifying the free joints of the chain.
947 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
949 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
952 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
954 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
957 IKFAST_API const char* GetKinematicsHash() { return "2062b4aa7fde98f002602493c9ba542a"; }
959 IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; }
961 #ifdef IKFAST_NAMESPACE
965 #ifndef IKFAST_NO_MAIN
968 #ifdef IKFAST_NAMESPACE
969 using namespace IKFAST_NAMESPACE;
971 int main(int argc, char** argv)
973 if( argc != 12+GetNumFreeParameters()+1 ) {
974 printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
975 "Returns the ik solutions given the transformation of the end effector specified by\n"
976 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
977 "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
981 IkSolutionList<IkReal> solutions;
982 std::vector<IkReal> vfree(GetNumFreeParameters());
983 IkReal eerot[9],eetrans[3];
984 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
985 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
986 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
987 for(std::size_t i = 0; i < vfree.size(); ++i)
988 vfree[i] = atof(argv[13+i]);
989 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
992 fprintf(stderr,"Failed to get ik solution\n");
996 printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
997 std::vector<IkReal> solvalues(GetNumJoints());
998 for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
999 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
1000 printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
1001 std::vector<IkReal> vsolfree(sol.GetFree().size());
1002 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
1003 for( std::size_t j = 0; j < solvalues.size(); ++j)
1004 printf("%.15f, ", solvalues[j]);