ikfast/ik.cpp

   1 /// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE
   2 /// \author Rosen Diankov
   3 ///
   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
   8 ///
   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.
  14 ///
  15 /// ikfast version 0x10000049 generated on 2021-08-14 17:43:05.445798
  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;
  23
  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);
  27
  28 #include <cmath>
  29 #include <vector>
  30 #include <limits>
  31 #include <algorithm>
  32 #include <complex>
  33
  34 #ifndef IKFAST_ASSERT
  35 #include <stdexcept>
  36 #include <sstream>
  37 #include <iostream>
  38
  39 #ifdef _MSC_VER
  40 #ifndef __PRETTY_FUNCTION__
  41 #define __PRETTY_FUNCTION__ __FUNCDNAME__
  42 #endif
  43 #endif
  44
  45 #ifndef __PRETTY_FUNCTION__
  46 #define __PRETTY_FUNCTION__ __func__
  47 #endif
  48
  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()); } }
  50
  51 #endif
  52
  53 #if defined(_MSC_VER)
  54 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
  55 #else
  56 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
  57 #endif
  58
  59 #define IK2PI  ((IkReal)6.28318530717959)
  60 #define IKPI  ((IkReal)3.14159265358979)
  61 #define IKPI_2  ((IkReal)1.57079632679490)
  62
  63 #ifdef _MSC_VER
  64 #ifndef isnan
  65 #define isnan _isnan
  66 #endif
  67 #ifndef isinf
  68 #define isinf _isinf
  69 #endif
  70 //#ifndef isfinite
  71 //#define isfinite _isfinite
  72 //#endif
  73 #endif // _MSC_VER
  74
  75 // lapack routines
  76 extern "C" {
  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);
  83 }
  84
  85 using namespace std; // necessary to get std math routines
  86
  87 #ifdef IKFAST_NAMESPACE
  88 namespace IKFAST_NAMESPACE {
  89 #endif
  90
  91 inline float IKabs(float f) { return fabsf(f); }
  92 inline double IKabs(double f) { return fabs(f); }
  93
  94 inline float IKsqr(float f) { return f*f; }
  95 inline double IKsqr(double f) { return f*f; }
  96
  97 inline float IKlog(float f) { return logf(f); }
  98 inline double IKlog(double f) { return log(f); }
  99
 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)
 103 #endif
 104
 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)
 108 #endif
 109
 110 // minimum distance of separate solutions
 111 #ifndef IKFAST_SOLUTION_THRESH
 112 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
 113 #endif
 114
 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)
 118 #endif
 119
 120
 121 inline float IKasin(float f)
 122 {
 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);
 126 return asinf(f);
 127 }
 128 inline double IKasin(double f)
 129 {
 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;
 133 return asin(f);
 134 }
 135
 136 // return positive value in [0,y)
 137 inline float IKfmod(float x, float y)
 138 {
 139     while(x < 0) {
 140         x += y;
 141     }
 142     return fmodf(x,y);
 143 }
 144
 145 // return positive value in [0,y)
 146 inline double IKfmod(double x, double y)
 147 {
 148     while(x < 0) {
 149         x += y;
 150     }
 151     return fmod(x,y);
 152 }
 153
 154 inline float IKacos(float f)
 155 {
 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);
 159 return acosf(f);
 160 }
 161 inline double IKacos(double f)
 162 {
 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;
 166 return acos(f);
 167 }
 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);
 178 }
 179 inline float IKatan2(float fy, float fx) {
 180     if( isnan(fy) ) {
 181         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
 182         return float(IKPI_2);
 183     }
 184     else if( isnan(fx) ) {
 185         return 0;
 186     }
 187     return atan2f(fy,fx);
 188 }
 189 inline double IKatan2Simple(double fy, double fx) {
 190     return atan2(fy,fx);
 191 }
 192 inline double IKatan2(double fy, double fx) {
 193     if( isnan(fy) ) {
 194         IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
 195         return IKPI_2;
 196     }
 197     else if( isnan(fx) ) {
 198         return 0;
 199     }
 200     return atan2(fy,fx);
 201 }
 202
 203 template <typename T>
 204 struct CheckValue
 205 {
 206     T value;
 207     bool valid;
 208 };
 209
 210 template <typename T>
 211 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
 212 {
 213     CheckValue<T> ret;
 214     ret.valid = false;
 215     ret.value = 0;
 216     if( !isnan(fy) && !isnan(fx) ) {
 217         if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
 218             ret.value = IKatan2Simple(fy,fx);
 219             ret.valid = true;
 220         }
 221     }
 222     return ret;
 223 }
 224
 225 inline float IKsign(float f) {
 226     if( f > 0 ) {
 227         return float(1);
 228     }
 229     else if( f < 0 ) {
 230         return float(-1);
 231     }
 232     return 0;
 233 }
 234
 235 inline double IKsign(double f) {
 236     if( f > 0 ) {
 237         return 1.0;
 238     }
 239     else if( f < 0 ) {
 240         return -1.0;
 241     }
 242     return 0;
 243 }
 244
 245 template <typename T>
 246 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
 247 {
 248     CheckValue<T> ret;
 249     ret.valid = true;
 250     if( n == 0 ) {
 251         ret.value = 1.0;
 252         return ret;
 253     }
 254     else if( n == 1 )
 255     {
 256         ret.value = f;
 257         return ret;
 258     }
 259     else if( n < 0 )
 260     {
 261         if( f == 0 )
 262         {
 263             ret.valid = false;
 264             ret.value = (T)1.0e30;
 265             return ret;
 266         }
 267         if( n == -1 ) {
 268             ret.value = T(1.0)/f;
 269             return ret;
 270         }
 271     }
 272
 273     int num = n > 0 ? n : -n;
 274     if( num == 2 ) {
 275         ret.value = f*f;
 276     }
 277     else if( num == 3 ) {
 278         ret.value = f*f*f;
 279     }
 280     else {
 281         ret.value = 1.0;
 282         while(num>0) {
 283             if( num & 1 ) {
 284                 ret.value *= f;
 285             }
 286             num >>= 1;
 287             f *= f;
 288         }
 289     }
 290
 291     if( n < 0 ) {
 292         ret.value = T(1.0)/ret.value;
 293     }
 294     return ret;
 295 }
 296
 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;
 302 x0=IKcos(j[0]);
 303 x1=IKcos(j[2]);
 304 x2=IKcos(j[1]);
 305 x3=IKsin(j[1]);
 306 x4=IKsin(j[2]);
 307 x5=IKsin(j[3]);
 308 x6=IKsin(j[0]);
 309 x7=IKcos(j[3]);
 310 x8=((0.126)*x4);
 311 x9=((0.124)*x1);
 312 x10=((1.0)*x4);
 313 x11=((0.124)*x4);
 314 x12=((0.126)*x1);
 315 x13=(x2*x6);
 316 x14=(x1*x2);
 317 x15=(x0*x2);
 318 x16=(x0*x3);
 319 x17=(x1*x3);
 320 x18=(x3*x6);
 321 IkReal x19=((1.0)*x8);
 322 IkReal x20=((1.0)*x16);
 323 eetrans[0]=((0.012)+(((0.132)*x16))+((x7*(((((-1.0)*x16*x19))+((x12*x15))))))+(((-1.0)*x11*x20))+(((0.024)*x15))+((x5*(((((-1.0)*x15*x19))+(((-1.0)*x12*x20))))))+((x15*x9)));
 324 IkReal x21=((1.0)*x8);
 325 IkReal x22=((1.0)*x18);
 326 eetrans[1]=((((0.132)*x18))+((x7*(((((-1.0)*x18*x21))+((x12*x13))))))+(((-1.0)*x11*x22))+(((0.024)*x13))+((x5*(((((-1.0)*x13*x21))+(((-1.0)*x12*x22))))))+((x13*x9)));
 327 IkReal x23=((1.0)*x3);
 328 IkReal x24=((1.0)*x2);
 329 eetrans[2]=((0.0875)+(((-1.0)*x23*x9))+((x5*((((x3*x8))+(((-1.0)*x12*x24))))))+((x7*(((((-1.0)*x24*x8))+(((-1.0)*x12*x23))))))+(((0.132)*x2))+(((-1.0)*x11*x24))+(((-0.024)*x3)));
 330 IkReal x25=((1.0)*x10);
 331 if( ((((x5*(((((-1.0)*x17))+(((-1.0)*x2*x25))))))+((x7*(((((1.0)*x14))+(((-1.0)*x25*x3)))))))) < -1-IKFAST_SINCOS_THRESH || ((((x5*(((((-1.0)*x17))+(((-1.0)*x2*x25))))))+((x7*(((((1.0)*x14))+(((-1.0)*x25*x3)))))))) > 1+IKFAST_SINCOS_THRESH )
 332     continue;
 333 eerot[0]=IKacos((((x5*(((((-1.0)*x17))+(((-1.0)*x2*x25))))))+((x7*(((((1.0)*x14))+(((-1.0)*x25*x3))))))));
 334 return;
 335 }
 336 IKFAST_ASSERT(0);
 337 }
 338
 339 IKFAST_API int GetNumFreeParameters() { return 0; }
 340 IKFAST_API int* GetFreeParameters() { return NULL; }
 341 IKFAST_API int GetNumJoints() { return 4; }
 342
 343 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
 344
 345 IKFAST_API int GetIkType() { return 0x4400000d; }
 346
 347 class IKSolver {
 348 public:
 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;
 351
 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) {
 357     solutions.Clear();
 358 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
 359
 360 r00 = eerot[0];
 361 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
 362 new_px=px;
 363 new_py=py;
 364 new_pz=pz;
 365 new_r00=r00;
 366 r00 = new_r00; px = new_px; py = new_py; pz = new_pz;
 367
 368 pp=((px*px)+(py*py)+(pz*pz));
 369 {
 370 IkReal j0array[2], cj0array[2], sj0array[2];
 371 bool j0valid[2]={false};
 372 _nj0 = 2;
 373 CheckValue<IkReal> x27 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(((-0.012)+px)),IKFAST_ATAN2_MAGTHRESH);
 374 if(!x27.valid){
 375 continue;
 376 }
 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 )
 385 {
 386     j0array[0]-=IK2PI;
 387 }
 388 else if( j0array[0] < -IKPI )
 389 {    j0array[0]+=IK2PI;
 390 }
 391 j0valid[0] = true;
 392 if( j0array[1] > IKPI )
 393 {
 394     j0array[1]-=IK2PI;
 395 }
 396 else if( j0array[1] < -IKPI )
 397 {    j0array[1]+=IK2PI;
 398 }
 399 j0valid[1] = true;
 400 for(int ij0 = 0; ij0 < 2; ++ij0)
 401 {
 402 if( !j0valid[ij0] )
 403 {
 404     continue;
 405 }
 406 _ij0[0] = ij0; _ij0[1] = -1;
 407 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
 408 {
 409 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
 410 {
 411     j0valid[iij0]=false; _ij0[1] = iij0; break;
 412 }
 413 }
 414 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
 415
 416 IkReal op[162], zeror[48];
 417 int numroots;;
 418 IkReal x28=IKcos(j0);
 419 IkReal x29=((0.024)*px);
 420 IkReal x30=((0.439)*pz);
 421 IkReal x31=IKsin(j0);
 422 IkReal x32=((0.108872)+(((-0.496)*pz)));
 423 IkReal x33=((-0.2195)+pz);
 424 IkReal x34=((2.0)*pz);
 425 IkReal x35=((0.175)+(((-1.0)*x34)));
 426 IkReal x36=((0.089)*pz);
 427 IkReal x37=((0.496)*pz);
 428 IkReal x38=((-0.089)+(((-1.0)*x34)));
 429 IkReal x39=((-0.0445)+(((-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.439)+x34);
 438 IkReal x48=(py*x31);
 439 IkReal x49=(px*x28);
 440 IkReal x50=((0.006336)*x28);
 441 IkReal x51=((-0.175)+x34);
 442 IkReal x52=((0.000576)*x28);
 443 IkReal x53=((0.003552)*x28);
 444 IkReal x54=((0.022072)+x37);
 445 IkReal x55=((0.0024)*x28);
 446 IkReal x56=((0.048)*x48);
 447 IkReal x57=((0.048)*x49);
 448 IkReal x58=((0.528)*x49);
 449 IkReal x59=((0.528)*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=(x52+x29);
 456 IkReal x66=(x48+x49);
 457 IkReal x67=((((1.0)*x48))+(((1.0)*x49)));
 458 IkReal x68=(x62+x63);
 459 IkReal x69=(x60+x61);
 460 IkReal x70=(x57+x56);
 461 IkReal x71=(x59+x58);
 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.0084)+(((-0.096)*pz))+x71+(((-1.0)*x50)));
 473 IkReal x83=((0.0518)+(((-0.592)*pz))+x71+(((-1.0)*x50)));
 474 IkReal x84=((-0.035)+(((0.4)*pz))+x71+(((-1.0)*x50)));
 475 IkReal x85=(x70+x80);
 476 IkReal x86=((0.01359975)+x70+x64+(((-1.0)*x81)));
 477 IkReal x87=((-0.04889625)+x70+x64+(((-1.0)*x81)));
 478 IkReal x88=((-0.05435225)+(((-1.0)*x74))+x64+x69+(((-1.0)*x53)));
 479 IkReal x89=((-0.04244825)+(((-1.0)*x74))+x55+x64+(((-1.0)*x68)));
 480 IkReal x90=((0.05979975)+x65+(((-1.0)*x85)));
 481 IkReal x91=((-0.00269625)+x65+(((-1.0)*x85)));
 482 IkReal x92=((-0.00815225)+x53+x29+(((-1.0)*x69))+(((-1.0)*x80)));
 483 IkReal x93=((0.00375175)+x68+x29+(((-1.0)*x80))+(((-1.0)*x55)));
 484 op[0]=x86;
 485 op[1]=0;
 486 op[2]=x87;
 487 op[3]=x86;
 488 op[4]=0;
 489 op[5]=x87;
 490 op[6]=0;
 491 op[7]=0;
 492 op[8]=0;
 493 op[9]=x88;
 494 op[10]=x88;
 495 op[11]=0;
 496 op[12]=x32;
 497 op[13]=x32;
 498 op[14]=0;
 499 op[15]=x89;
 500 op[16]=x89;
 501 op[17]=0;
 502 op[18]=x45;
 503 op[19]=0;
 504 op[20]=x44;
 505 op[21]=0;
 506 op[22]=-4.0;
 507 op[23]=0;
 508 op[24]=x44;
 509 op[25]=0;
 510 op[26]=x45;
 511 op[27]=x78;
 512 op[28]=0;
 513 op[29]=x79;
 514 op[30]=0;
 515 op[31]=-0.504;
 516 op[32]=0;
 517 op[33]=((-0.226)+x46+(((-1.0)*x67)));
 518 op[34]=0;
 519 op[35]=((0.026)+x46+(((-1.0)*x67)));
 520 op[36]=x78;
 521 op[37]=0;
 522 op[38]=x79;
 523 op[39]=x47;
 524 op[40]=0;
 525 op[41]=x47;
 526 op[42]=((0.226)+(((-1.0)*x46))+x66);
 527 op[43]=0;
 528 op[44]=((-0.026)+(((-1.0)*x46))+x66);
 529 op[45]=x33;
 530 op[46]=0.252;
 531 op[47]=x33;
 532 op[48]=0.5;
 533 op[49]=0;
 534 op[50]=-0.004;
 535 op[51]=x33;
 536 op[52]=-0.252;
 537 op[53]=x33;
 538 op[54]=x82;
 539 op[55]=0;
 540 op[56]=x82;
 541 op[57]=x82;
 542 op[58]=0;
 543 op[59]=x82;
 544 op[60]=0;
 545 op[61]=0;
 546 op[62]=0;
 547 op[63]=x83;
 548 op[64]=x83;
 549 op[65]=0;
 550 op[66]=x75;
 551 op[67]=x75;
 552 op[68]=0;
 553 op[69]=x84;
 554 op[70]=x84;
 555 op[71]=0;
 556 op[72]=0;
 557 op[73]=-4.0;
 558 op[74]=0;
 559 op[75]=-4.0;
 560 op[76]=0;
 561 op[77]=4.0;
 562 op[78]=0;
 563 op[79]=4.0;
 564 op[80]=0;
 565 op[81]=x51;
 566 op[82]=0;
 567 op[83]=x51;
 568 op[84]=0;
 569 op[85]=0;
 570 op[86]=0;
 571 op[87]=x51;
 572 op[88]=0;
 573 op[89]=x51;
 574 op[90]=x51;
 575 op[91]=0;
 576 op[92]=x51;
 577 op[93]=x76;
 578 op[94]=0;
 579 op[95]=x76;
 580 op[96]=x35;
 581 op[97]=0;
 582 op[98]=x35;
 583 op[99]=x77;
 584 op[100]=0;
 585 op[101]=x77;
 586 op[102]=0;
 587 op[103]=0;
 588 op[104]=0;
 589 op[105]=x77;
 590 op[106]=0;
 591 op[107]=x77;
 592 op[108]=x90;
 593 op[109]=0;
 594 op[110]=x91;
 595 op[111]=x90;
 596 op[112]=0;
 597 op[113]=x91;
 598 op[114]=0;
 599 op[115]=0;
 600 op[116]=0;
 601 op[117]=x92;
 602 op[118]=x92;
 603 op[119]=0;
 604 op[120]=x54;
 605 op[121]=x54;
 606 op[122]=0;
 607 op[123]=x93;
 608 op[124]=x93;
 609 op[125]=0;
 610 op[126]=x44;
 611 op[127]=0;
 612 op[128]=x45;
 613 op[129]=0;
 614 op[130]=4.0;
 615 op[131]=0;
 616 op[132]=x45;
 617 op[133]=0;
 618 op[134]=x44;
 619 op[135]=x72;
 620 op[136]=0;
 621 op[137]=x73;
 622 op[138]=0;
 623 op[139]=-0.504;
 624 op[140]=0;
 625 op[141]=((-0.226)+(((-1.0)*x46))+x66);
 626 op[142]=0;
 627 op[143]=((0.026)+(((-1.0)*x46))+x66);
 628 op[144]=x72;
 629 op[145]=0;
 630 op[146]=x73;
 631 op[147]=x38;
 632 op[148]=0;
 633 op[149]=x38;
 634 op[150]=((0.226)+x46+(((-1.0)*x67)));
 635 op[151]=0;
 636 op[152]=((-0.026)+x46+(((-1.0)*x67)));
 637 op[153]=x39;
 638 op[154]=0.252;
 639 op[155]=x39;
 640 op[156]=0.5;
 641 op[157]=0;
 642 op[158]=-0.004;
 643 op[159]=x39;
 644 op[160]=-0.252;
 645 op[161]=x39;
 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)
 650 {
 651 IkReal htj1 = zeror[ij1+0], htj2 = zeror[ij1+1], htj3 = zeror[ij1+2];
 652 if(isnan(htj1)||isnan(htj2)||isnan(htj3)){
 653 continue;
 654 }
 655 j1array[numsolutions]=((2.0)*(atan(htj1)));
 656 j2array[numsolutions]=((2.0)*(atan(htj2)));
 657 j3array[numsolutions]=((2.0)*(atan(htj3)));
 658 if(isinf(htj1)){
 659 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
 660 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
 661 }
 662 else{
 663 IkReal x94=htj1*htj1;
 664 CheckValue<IkReal> x95=IKPowWithIntegerCheck(((1.0)+x94),-1);
 665 if(!x95.valid){
 666 continue;
 667 }
 668 cj1array[numsolutions]=((x95.value)*(((1.0)+(((-1.0)*x94)))));
 669 CheckValue<IkReal> x96=IKPowWithIntegerCheck(((1.0)+(htj1*htj1)),-1);
 670 if(!x96.valid){
 671 continue;
 672 }
 673 sj1array[numsolutions]=((2.0)*htj1*(x96.value));
 674 }
 675 if(isinf(htj2)){
 676 cj2array[numsolutions] = IKcos(j2array[numsolutions]);
 677 sj2array[numsolutions] = IKsin(j2array[numsolutions]);
 678 }
 679 else{
 680 IkReal x97=htj2*htj2;
 681 CheckValue<IkReal> x98=IKPowWithIntegerCheck(((1.0)+x97),-1);
 682 if(!x98.valid){
 683 continue;
 684 }
 685 cj2array[numsolutions]=((x98.value)*(((1.0)+(((-1.0)*x97)))));
 686 CheckValue<IkReal> x99=IKPowWithIntegerCheck(((1.0)+(htj2*htj2)),-1);
 687 if(!x99.valid){
 688 continue;
 689 }
 690 sj2array[numsolutions]=((2.0)*htj2*(x99.value));
 691 }
 692 if(isinf(htj3)){
 693 cj3array[numsolutions] = IKcos(j3array[numsolutions]);
 694 sj3array[numsolutions] = IKsin(j3array[numsolutions]);
 695 }
 696 else{
 697 IkReal x100=htj3*htj3;
 698 CheckValue<IkReal> x101=IKPowWithIntegerCheck(((1.0)+x100),-1);
 699 if(!x101.valid){
 700 continue;
 701 }
 702 cj3array[numsolutions]=((x101.value)*(((1.0)+(((-1.0)*x100)))));
 703 CheckValue<IkReal> x102=IKPowWithIntegerCheck(((1.0)+(htj3*htj3)),-1);
 704 if(!x102.valid){
 705 continue;
 706 }
 707 sj3array[numsolutions]=((2.0)*htj3*(x102.value));
 708 }
 709 if( j1array[numsolutions] > IKPI )
 710 {
 711     j1array[numsolutions]-=IK2PI;
 712 }
 713 else if( j1array[numsolutions] < -IKPI )
 714 {
 715     j1array[numsolutions]+=IK2PI;
 716 }
 717 if( j2array[numsolutions] > IKPI )
 718 {
 719     j2array[numsolutions]-=IK2PI;
 720 }
 721 else if( j2array[numsolutions] < -IKPI )
 722 {
 723     j2array[numsolutions]+=IK2PI;
 724 }
 725 if( j3array[numsolutions] > IKPI )
 726 {
 727     j3array[numsolutions]-=IK2PI;
 728 }
 729 else if( j3array[numsolutions] < -IKPI )
 730 {
 731     j3array[numsolutions]+=IK2PI;
 732 }
 733 numsolutions++;
 734 }
 735 bool j1valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
 736 _nj1 = 16;
 737 _nj2 = 1;
 738 _nj3 = 1;
 739 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
 740     {
 741 if( !j1valid[ij1] )
 742 {
 743     continue;
 744 }
 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)
 749 {
 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 )
 752 {
 753     j1valid[iij1]=false; _ij1[1] = iij1; _ij2[1] = 0; _ij3[1] = 0;  break;
 754 }
 755 }
 756     j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
 757
 758     j2 = j2array[ij1]; cj2 = cj2array[ij1]; sj2 = sj2array[ij1];
 759
 760     j3 = j3array[ij1]; cj3 = cj3array[ij1]; sj3 = sj3array[ij1];
 761
 762 {
 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);
 786 }
 787     }
 788 }
 789 }
 790 }
 791 return solutions.GetNumSolutions()>0;
 792 }
 793
 794 /// \brief Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994)
 795 ///
 796 /// matcoeffs is of length 54*3, for 3 matrices
 797 static inline void solvedialyticpoly12qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
 798 {
 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]);
 803     int ipiv[12];
 804     int info, coeffindex;
 805     const int worksize=24*24*23;
 806     const int matrixdim = 12;
 807     const int matrixdim2 = 24;
 808     numroots = 0;
 809     // first setup M = [0 I; -C -B] and A
 810     coeffindex = 0;
 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++];
 814         }
 815     }
 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++];
 819         }
 820     }
 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++];
 824         }
 825         for(int k = 0; k < 3; ++k) {
 826             A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
 827         }
 828     }
 829     const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
 830     int lfindex = -1;
 831     bool bsingular = true;
 832     do {
 833         dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
 834         if( info == 0 ) {
 835             bsingular = false;
 836             for(int j = 0; j < matrixdim; ++j) {
 837                 if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
 838                     bsingular = true;
 839                     break;
 840                 }
 841             }
 842             if( !bsingular ) {
 843                 break;
 844             }
 845         }
 846         if( lfindex == 3 ) {
 847             break;
 848         }
 849         // transform by the linear functional
 850         lfindex++;
 851         const IkReal* lf = lfpossibilities[lfindex];
 852         // have to reinitialize A
 853         coeffindex = 0;
 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);
 860                 coeffindex++;
 861             }
 862             for(int k = 0; k < 3; ++k) {
 863                 A[j+matrixdim*k] = A[(j+6)+matrixdim*(k+9)] = 0;
 864             }
 865         }
 866     } while(lfindex<4);
 867
 868     if( bsingular ) {
 869         return;
 870     }
 871     dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
 872     if( info != 0 ) {
 873         return;
 874     }
 875
 876     // set identity in upper corner
 877     for(int j = 0; j < matrixdim; ++j) {
 878         M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
 879     }
 880     IkReal IKFAST_ALIGNED16(wr[24]);
 881     IkReal IKFAST_ALIGNED16(wi[24]);
 882     IkReal IKFAST_ALIGNED16(vr[24*24]);
 883     int one=1;
 884     dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
 885     if( info != 0 ) {
 886         return;
 887     }
 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 ) {
 893                 ev += matrixdim;
 894             }
 895             // consistency has to be checked!!
 896             if( IKabs(ev[0]) < tol ) {
 897                 continue;
 898             }
 899             IkReal iconst = 1/ev[0];
 900             for(int j = 1; j < matrixdim; ++j) {
 901                 Breal[j-1] = ev[j]*iconst;
 902             }
 903             if( checkconsistency12(Breal) ) {
 904                 if( lfindex >= 0 ) {
 905                     const IkReal* lf = lfpossibilities[lfindex];
 906                     rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
 907                 }
 908                 else {
 909                     rawroots[numroots++] = wr[i];
 910                 }
 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];
 916                 }
 917                 else if( bsmall0 && !bsmall1 ) {
 918                     rawroots[numroots++] = ev[5]/ev[2];
 919                     rawroots[numroots++] = ev[2]/ev[1];
 920                 }
 921                 else if( !bsmall0 && bsmall1 ) {
 922                     rawroots[numroots++] = ev[9]/ev[6];
 923                     rawroots[numroots++] = ev[10]/ev[9];
 924                 }
 925                 else if( !bsmall0 && !bsmall1 ) {
 926                     rawroots[numroots++] = ev[11]/ev[8];
 927                     rawroots[numroots++] = ev[11]/ev[10];
 928                 }
 929             }
 930         }
 931     }
 932 }
 933 static inline bool checkconsistency12(const IkReal* Breal)
 934 {
 935     IkReal norm = 0.1;
 936     for(int i = 0; i < 11; ++i) {
 937         norm += IKabs(Breal[i]);
 938     }
 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;
 941 }
 942 };
 943
 944
 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) {
 948 IKSolver solver;
 949 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
 950 }
 951
 952 IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
 953 IKSolver solver;
 954 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
 955 }
 956
 957 IKFAST_API const char* GetKinematicsHash() { return "f8e185c58eba9ecf036d09a0db9b2f5b"; }
 958
 959 IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; }
 960
 961 #ifdef IKFAST_NAMESPACE
 962 } // end namespace
 963 #endif
 964
 965 #ifndef IKFAST_NO_MAIN
 966 #include <stdio.h>
 967 #include <stdlib.h>
 968 #ifdef IKFAST_NAMESPACE
 969 using namespace IKFAST_NAMESPACE;
 970 #endif
 971 int main(int argc, char** argv)
 972 {
 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());
 978         return 1;
 979     }
 980
 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);
 990
 991     if( !bSuccess ) {
 992         fprintf(stderr,"Failed to get ik solution\n");
 993         return -1;
 994     }
 995
 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]);
1005         printf("\n");
1006     }
1007     return 0;
1008 }
1009
1010 #endif