duaro_lower_arm_ikfast_solver.cpp

Go to the documentation of this file.

#define IKFAST_HAS_LIBRARY
#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
using namespace ikfast;

// check if the included ikfast version matches what this file was compiled with
#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x1000004a);

#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>
#include <complex>

#ifndef IKFAST_ASSERT
#include <stdexcept>
#include <sstream>
#include <iostream>

#ifdef _MSC_VER
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __FUNCDNAME__
#endif
#endif

#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __func__
#endif

#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()); } }

#endif

#if defined(_MSC_VER)
#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
#else
#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
#endif

#define IK2PI  ((IkReal)6.28318530717959)
#define IKPI  ((IkReal)3.14159265358979)
#define IKPI_2  ((IkReal)1.57079632679490)

#ifdef _MSC_VER
#ifndef isnan
#define isnan _isnan
#endif
#ifndef isinf
#define isinf _isinf
#endif
//#ifndef isfinite
//#define isfinite _isfinite
//#endif
#endif // _MSC_VER

// lapack routines
extern "C" {
  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
  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);
  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);
}

using namespace std; // necessary to get std math routines

#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE {
#endif

inline float IKabs(float f) { return fabsf(f); }
inline double IKabs(double f) { return fabs(f); }

inline float IKsqr(float f) { return f*f; }
inline double IKsqr(double f) { return f*f; }

inline float IKlog(float f) { return logf(f); }
inline double IKlog(double f) { return log(f); }

// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
#ifndef IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
#endif

// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
#ifndef IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
#endif

// minimum distance of separate solutions
#ifndef IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
#endif

// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
#ifndef IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
#endif


inline float IKasin(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(-IKPI_2);
else if( f >= 1 ) return float(IKPI_2);
return asinf(f);
}
inline double IKasin(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return -IKPI_2;
else if( f >= 1 ) return IKPI_2;
return asin(f);
}

// return positive value in [0,y)
inline float IKfmod(float x, float y)
{
    while(x < 0) {
        x += y;
    }
    return fmodf(x,y);
}

// return positive value in [0,y)
inline double IKfmod(double x, double y)
{
    while(x < 0) {
        x += y;
    }
    return fmod(x,y);
}

inline float IKacos(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(IKPI);
else if( f >= 1 ) return float(0);
return acosf(f);
}
inline double IKacos(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return IKPI;
else if( f >= 1 ) return 0;
return acos(f);
}
inline float IKsin(float f) { return sinf(f); }
inline double IKsin(double f) { return sin(f); }
inline float IKcos(float f) { return cosf(f); }
inline double IKcos(double f) { return cos(f); }
inline float IKtan(float f) { return tanf(f); }
inline double IKtan(double f) { return tan(f); }
inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
inline float IKatan2Simple(float fy, float fx) {
    return atan2f(fy,fx);
}
inline float IKatan2(float fy, float fx) {
    if( isnan(fy) ) {
        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
        return float(IKPI_2);
    }
    else if( isnan(fx) ) {
        return 0;
    }
    return atan2f(fy,fx);
}
inline double IKatan2Simple(double fy, double fx) {
    return atan2(fy,fx);
}
inline double IKatan2(double fy, double fx) {
    if( isnan(fy) ) {
        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
        return IKPI_2;
    }
    else if( isnan(fx) ) {
        return 0;
    }
    return atan2(fy,fx);
}

template <typename T>
struct CheckValue
{
    T value;
    bool valid;
};

template <typename T>
inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
{
    CheckValue<T> ret;
    ret.valid = false;
    ret.value = 0;
    if( !isnan(fy) && !isnan(fx) ) {
        if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
            ret.value = IKatan2Simple(fy,fx);
            ret.valid = true;
        }
    }
    return ret;
}

inline float IKsign(float f) {
    if( f > 0 ) {
        return float(1);
    }
    else if( f < 0 ) {
        return float(-1);
    }
    return 0;
}

inline double IKsign(double f) {
    if( f > 0 ) {
        return 1.0;
    }
    else if( f < 0 ) {
        return -1.0;
    }
    return 0;
}

template <typename T>
inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
{
    CheckValue<T> ret;
    ret.valid = true;
    if( n == 0 ) {
        ret.value = 1.0;
        return ret;
    }
    else if( n == 1 )
    {
        ret.value = f;
        return ret;
    }
    else if( n < 0 )
    {
        if( f == 0 )
        {
            ret.valid = false;
            ret.value = (T)1.0e30;
            return ret;
        }
        if( n == -1 ) {
            ret.value = T(1.0)/f;
            return ret;
        }
    }

    int num = n > 0 ? n : -n;
    if( num == 2 ) {
        ret.value = f*f;
    }
    else if( num == 3 ) {
        ret.value = f*f*f;
    }
    else {
        ret.value = 1.0;
        while(num>0) {
            if( num & 1 ) {
                ret.value *= f;
            }
            num >>= 1;
            f *= f;
        }
    }
    
    if( n < 0 ) {
        ret.value = T(1.0)/ret.value;
    }
    return ret;
}

IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11;
x0=IKsin(j[0]);
x1=IKcos(j[0]);
x2=IKcos(j[1]);
x3=IKsin(j[1]);
x4=IKcos(j[3]);
x5=IKsin(j[3]);
x6=((0.4)*x0);
x7=((0.4)*x1);
x8=((1.0)*x2);
x9=((1.0)*x3);
x10=(x0*x9);
x11=(((x0*x8))+((x1*x9)));
eetrans[0]=((((-0.36)*x0))+(((-1.0)*x2*x6))+(((-1.0)*x3*x7)));
eetrans[1]=(((x2*x7))+(((-1.0)*x3*x6))+(((0.36)*x1)));
eetrans[2]=((0.1405)+j[2]);
IkReal x12=((1.0)*x10);
CheckValue<IkReal> x13 = IKatan2WithCheck(IkReal((((x5*(((((-1.0)*x12))+((x1*x8))))))+((x11*x4)))),IkReal(((((-1.0)*x11*x5))+((x4*(((((-1.0)*x12))+((x1*x2)))))))),IKFAST_ATAN2_MAGTHRESH);
if(!x13.valid){
continue;
}
eerot[0]=x13.value;
return;
}
IKFAST_ASSERT(0);
}

IKFAST_API int GetNumFreeParameters() { return 0; }
IKFAST_API int* GetFreeParameters() { return NULL; }
IKFAST_API int GetNumJoints() { return 4; }

IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }

IKFAST_API int GetIkType() { return 0x4400000e; }

class IKSolver {
public:
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;
unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3;

IkReal j100, cj100, sj100;
unsigned char _ij100[2], _nj100;
bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
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; 
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
    solutions.Clear();
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];

r00 = eerot[0];
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
new_px=px;
new_py=py;
new_pz=pz;
new_r00=r00;
r00 = new_r00; px = new_px; py = new_py; pz = new_pz;

pp=((px*px)+(py*py)+(pz*pz));
{
IkReal verifyeval[1];
verifyeval[0]=0;
if( IKabs(verifyeval[0]) < 0.0000010000000000  )
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
j2array[0]=((-0.1405)+pz);
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];

{
IkReal j0eval[2];
j0eval[0]=((IKabs(px))+(IKabs(py)));
j0eval[1]=((px*px)+(py*py));
if( IKabs(j0eval[0]) < 0.0000010000000000  || IKabs(j0eval[1]) < 0.0000010000000000  )
{
continue; // no branches [j0, j1]

} else
{
{
IkReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
IkReal x14=px*px;
IkReal x15=py*py;
CheckValue<IkReal> x18 = IKatan2WithCheck(IkReal(((0.72)*py)),IkReal(((-0.72)*px)),IKFAST_ATAN2_MAGTHRESH);
if(!x18.valid){
continue;
}
IkReal x16=((1.0)*(x18.value));
if((((((0.5184)*x14))+(((0.5184)*x15)))) < -0.00001)
continue;
CheckValue<IkReal> x19=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.5184)*x14))+(((0.5184)*x15))))),-1);
if(!x19.valid){
continue;
}
if( (((x19.value)*(((0.01065975)+(((-1.0)*x15))+(((-1.0)*x14))+(((-1.0)*(pz*pz)))+(((0.281)*pz))+(j2*j2))))) < -1-IKFAST_SINCOS_THRESH || (((x19.value)*(((0.01065975)+(((-1.0)*x15))+(((-1.0)*x14))+(((-1.0)*(pz*pz)))+(((0.281)*pz))+(j2*j2))))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IkReal x17=IKasin(((x19.value)*(((0.01065975)+(((-1.0)*x15))+(((-1.0)*x14))+(((-1.0)*(pz*pz)))+(((0.281)*pz))+(j2*j2)))));
j0array[0]=((((-1.0)*x16))+(((-1.0)*x17)));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((3.14159265358979)+(((-1.0)*x16))+x17);
sj0array[1]=IKsin(j0array[1]);
cj0array[1]=IKcos(j0array[1]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
if( j0array[1] > IKPI )
{
    j0array[1]-=IK2PI;
}
else if( j0array[1] < -IKPI )
{    j0array[1]+=IK2PI;
}
j0valid[1] = true;
for(int ij0 = 0; ij0 < 2; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 2; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
{
IkReal evalcond[1];
evalcond[0]=((0.05014025)+(((-1.0)*(px*px)))+(((-0.72)*px*(IKsin(j0))))+(((-1.0)*(pz*pz)))+(((2.0)*j2*pz))+(((0.72)*py*(IKcos(j0))))+(((-1.0)*(py*py)))+(((-1.0)*(j2*j2))));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j1eval[3];
IkReal x20=px*px;
IkReal x21=py*py;
IkReal x22=(cj0*py);
IkReal x23=(px*sj0);
j1eval[0]=((1.0)+(((7.71604938271605)*x21))+(((7.71604938271605)*x20))+(((5.55555555555556)*x23))+(((-5.55555555555556)*x22)));
j1eval[1]=IKsign(((0.648)+(((5.0)*x21))+(((5.0)*x20))+(((3.6)*x23))+(((-3.6)*x22))));
j1eval[2]=((IKabs(((-0.72)+(((-2.0)*x23))+(((2.0)*x22)))))+(IKabs(((((-2.0)*py*sj0))+(((-2.0)*cj0*px))))));
if( IKabs(j1eval[0]) < 0.0000010000000000  || IKabs(j1eval[1]) < 0.0000010000000000  || IKabs(j1eval[2]) < 0.0000010000000000  )
{
{
IkReal evalcond[1];
bool bgotonextstatement = true;
do
{
IkReal x24=py*py;
IkReal x25=px*px;
IkReal x26=((((30.8641975308642)*x24))+(((30.8641975308642)*x25)));
IkReal x27=((1.0)+(((7.71604938271605)*x24))+(((7.71604938271605)*x25)));
if((x26) < -0.00001)
continue;
IkReal x28=IKabs(IKsqrt(x26));
IkReal x35 = x26;
if(IKabs(x35)==0){
continue;
}
IkReal x29=pow(x35,-0.5);
CheckValue<IkReal> x36=IKPowWithIntegerCheck(x28,-1);
if(!x36.valid){
continue;
}
IkReal x30=x36.value;
IkReal x31=((5.55555555555556)*px*x29);
IkReal x32=((5.55555555555556)*py*x29);
IkReal x33=(x27*x30);
if((((1.0)+(((-1.0)*(x33*x33))))) < -0.00001)
continue;
IkReal x34=IKsqrt(((1.0)+(((-1.0)*(x33*x33)))));
if( (x33) < -1-IKFAST_SINCOS_THRESH || (x33) > 1+IKFAST_SINCOS_THRESH )
    continue;
CheckValue<IkReal> x37 = IKatan2WithCheck(IkReal(((-5.55555555555556)*py)),IkReal(((5.55555555555556)*px)),IKFAST_ATAN2_MAGTHRESH);
if(!x37.valid){
continue;
}
IkReal gconst0=((((-1.0)*(IKasin(x33))))+(((-1.0)*(x37.value))));
IkReal gconst1=((((-1.0)*x31*x33))+((x32*x34)));
IkReal gconst2=(((x32*x33))+((x31*x34)));
IkReal x38=py*py;
IkReal x39=px*px;
if((((((30.8641975308642)*x39))+(((30.8641975308642)*x38)))) < -0.00001)
continue;
CheckValue<IkReal> x42=IKPowWithIntegerCheck(IKabs(IKsqrt(((((30.8641975308642)*x39))+(((30.8641975308642)*x38))))),-1);
if(!x42.valid){
continue;
}
IkReal x40=x42.value;
IkReal x41=((7.71604938271605)*x40);
if( ((((x38*x41))+((x39*x41))+x40)) < -1-IKFAST_SINCOS_THRESH || ((((x38*x41))+((x39*x41))+x40)) > 1+IKFAST_SINCOS_THRESH )
    continue;
CheckValue<IkReal> x43 = IKatan2WithCheck(IkReal(((-5.55555555555556)*py)),IkReal(((5.55555555555556)*px)),IKFAST_ATAN2_MAGTHRESH);
if(!x43.valid){
continue;
}
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((IKasin((((x38*x41))+((x39*x41))+x40)))+(x43.value)+j0)))), 6.28318530717959)));
if( IKabs(evalcond[0]) < 0.0000050000000000  )
{
bgotonextstatement=false;
{
IkReal j1array[2], cj1array[2], sj1array[2];
bool j1valid[2]={false};
_nj1 = 2;
IkReal x44=((1.0)*gconst2);
CheckValue<IkReal> x46 = IKatan2WithCheck(IkReal(((((-1.0)*gconst1*py))+(((-1.0)*px*x44)))),IkReal(((0.36)+((gconst1*px))+(((-1.0)*py*x44)))),IKFAST_ATAN2_MAGTHRESH);
if(!x46.valid){
continue;
}
IkReal x45=x46.value;
j1array[0]=((-1.0)*x45);
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
j1array[1]=((3.14159265358979)+(((-1.0)*x45)));
sj1array[1]=IKsin(j1array[1]);
cj1array[1]=IKcos(j1array[1]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
if( j1array[1] > IKPI )
{
    j1array[1]-=IK2PI;
}
else if( j1array[1] < -IKPI )
{    j1array[1]+=IK2PI;
}
j1valid[1] = true;
for(int ij1 = 0; ij1 < 2; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < 2; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
{
IkReal evalcond[1];
IkReal x47=IKsin(j1);
IkReal x48=IKcos(j1);
evalcond[0]=((0.4)+(((-1.0)*gconst2*py*x48))+((gconst1*px*x48))+((gconst2*px*x47))+((gconst1*py*x47))+(((0.36)*x48)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
j3array[0]=(r00+(((-1.0)*j0))+(((-1.0)*j1)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(4);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 17;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}

}
} while(0);
if( bgotonextstatement )
{
bool bgotonextstatement = true;
do
{
IkReal x49=py*py;
IkReal x50=px*px;
IkReal x51=((5.55555555555556)*px);
IkReal x52=((((30.8641975308642)*x50))+(((30.8641975308642)*x49)));
IkReal x53=((1.0)+(((7.71604938271605)*x49))+(((7.71604938271605)*x50)));
IkReal x60 = x52;
if(IKabs(x60)==0){
continue;
}
IkReal x54=pow(x60,-0.5);
if((x52) < -0.00001)
continue;
IkReal x55=IKabs(IKsqrt(x52));
CheckValue<IkReal> x61=IKPowWithIntegerCheck(x55,-1);
if(!x61.valid){
continue;
}
IkReal x56=x61.value;
IkReal x57=(x53*x56);
if((((1.0)+(((-1.0)*(x57*x57))))) < -0.00001)
continue;
IkReal x58=IKsqrt(((1.0)+(((-1.0)*(x57*x57)))));
IkReal x59=((5.55555555555556)*x54*x58);
if( (x57) < -1-IKFAST_SINCOS_THRESH || (x57) > 1+IKFAST_SINCOS_THRESH )
    continue;
CheckValue<IkReal> x62 = IKatan2WithCheck(IkReal(((-5.55555555555556)*py)),IkReal(x51),IKFAST_ATAN2_MAGTHRESH);
if(!x62.valid){
continue;
}
IkReal gconst3=((3.14159265358979)+(IKasin(x57))+(((-1.0)*(x62.value))));
IkReal gconst4=((((-1.0)*x51*x54*x57))+(((-1.0)*py*x59)));
IkReal gconst5=((((5.55555555555556)*py*x54*x57))+(((-1.0)*x51*x54*x58)));
IkReal x63=py*py;
IkReal x64=px*px;
if((((((30.8641975308642)*x63))+(((30.8641975308642)*x64)))) < -0.00001)
continue;
CheckValue<IkReal> x65=IKPowWithIntegerCheck(IKabs(IKsqrt(((((30.8641975308642)*x63))+(((30.8641975308642)*x64))))),-1);
if(!x65.valid){
continue;
}
if( (((x65.value)*(((1.0)+(((7.71604938271605)*x64))+(((7.71604938271605)*x63)))))) < -1-IKFAST_SINCOS_THRESH || (((x65.value)*(((1.0)+(((7.71604938271605)*x64))+(((7.71604938271605)*x63)))))) > 1+IKFAST_SINCOS_THRESH )
    continue;
CheckValue<IkReal> x66 = IKatan2WithCheck(IkReal(((-5.55555555555556)*py)),IkReal(((5.55555555555556)*px)),IKFAST_ATAN2_MAGTHRESH);
if(!x66.valid){
continue;
}
evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(((-1.0)*(IKasin(((x65.value)*(((1.0)+(((7.71604938271605)*x64))+(((7.71604938271605)*x63)))))))))+(x66.value)+j0)))), 6.28318530717959)));
if( IKabs(evalcond[0]) < 0.0000050000000000  )
{
bgotonextstatement=false;
{
IkReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
_nj1 = 1;
IkReal x67=px*px;
IkReal x68=gconst4*gconst4;
IkReal x69=gconst5*gconst5;
IkReal x70=py*py;
IkReal x71=((2.0)*gconst4);
IkReal x72=((2.0)*gconst5);
IkReal x73=((5.0)*x70);
IkReal x74=((5.0)*x67);
CheckValue<IkReal> x75 = IKatan2WithCheck(IkReal(((((-1.0)*px*x72))+(((-1.0)*py*x71)))),IkReal(((-0.72)+((py*x72))+(((-1.0)*px*x71)))),IKFAST_ATAN2_MAGTHRESH);
if(!x75.valid){
continue;
}
CheckValue<IkReal> x76=IKPowWithIntegerCheck(IKsign(((0.648)+((x69*x74))+((x69*x73))+((x68*x73))+((x68*x74))+(((-3.6)*gconst5*py))+(((3.6)*gconst4*px)))),-1);
if(!x76.valid){
continue;
}
j1array[0]=((-1.5707963267949)+(x75.value)+(((1.5707963267949)*(x76.value))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < 1; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
{
IkReal evalcond[2];
IkReal x77=IKsin(j1);
IkReal x78=IKcos(j1);
IkReal x79=((1.0)*gconst5);
IkReal x80=(gconst4*x77);
IkReal x81=(px*x78);
IkReal x82=(py*x78);
evalcond[0]=((0.4)+(((0.36)*x78))+(((-1.0)*x79*x82))+((py*x80))+((gconst4*x81))+((gconst5*px*x77)));
evalcond[1]=((((0.36)*x77))+(((-1.0)*x79*x81))+((px*x80))+(((-1.0)*gconst4*x82))+(((-1.0)*py*x77*x79)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
j3array[0]=(r00+(((-1.0)*j0))+(((-1.0)*j1)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(4);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 17;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}

}
} while(0);
if( bgotonextstatement )
{
bool bgotonextstatement = true;
do
{
if( 1 )
{
bgotonextstatement=false;
continue; // branch miss [j1]

}
} while(0);
if( bgotonextstatement )
{
}
}
}
}

} else
{
{
IkReal j1array[1], cj1array[1], sj1array[1];
bool j1valid[1]={false};
_nj1 = 1;
IkReal x83=((2.0)*cj0);
IkReal x84=(px*sj0);
CheckValue<IkReal> x85 = IKatan2WithCheck(IkReal(((((-2.0)*py*sj0))+(((-1.0)*px*x83)))),IkReal(((-0.72)+((py*x83))+(((-2.0)*x84)))),IKFAST_ATAN2_MAGTHRESH);
if(!x85.valid){
continue;
}
CheckValue<IkReal> x86=IKPowWithIntegerCheck(IKsign(((0.648)+(((-3.6)*cj0*py))+(((5.0)*(px*px)))+(((5.0)*(py*py)))+(((3.6)*x84)))),-1);
if(!x86.valid){
continue;
}
j1array[0]=((-1.5707963267949)+(x85.value)+(((1.5707963267949)*(x86.value))));
sj1array[0]=IKsin(j1array[0]);
cj1array[0]=IKcos(j1array[0]);
if( j1array[0] > IKPI )
{
    j1array[0]-=IK2PI;
}
else if( j1array[0] < -IKPI )
{    j1array[0]+=IK2PI;
}
j1valid[0] = true;
for(int ij1 = 0; ij1 < 1; ++ij1)
{
if( !j1valid[ij1] )
{
    continue;
}
_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < 1; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
{
IkReal evalcond[2];
IkReal x87=IKcos(j1);
IkReal x88=IKsin(j1);
IkReal x89=(py*sj0);
IkReal x90=(px*sj0);
IkReal x91=(cj0*px);
IkReal x92=(cj0*py);
IkReal x93=((1.0)*x87);
evalcond[0]=((0.4)+((x87*x90))+((x88*x91))+((x88*x89))+(((-1.0)*x92*x93))+(((0.36)*x87)));
evalcond[1]=((((-1.0)*x91*x93))+(((-1.0)*x88*x92))+((x88*x90))+(((0.36)*x88))+(((-1.0)*x89*x93)));
if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH  || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH  )
{
continue;
}
}

{
IkReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
j3array[0]=(r00+(((-1.0)*j0))+(((-1.0)*j1)));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(4);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 17;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}
}
}

}

}
}
}

}

}
}
}

} else
{
continue; // verifyAllEquations

}

}
}
return solutions.GetNumSolutions()>0;
}
};


IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
IKSolver solver;
return solver.ComputeIk(eetrans,eerot,pfree,solutions);
}

IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip) {
IKSolver solver;
return solver.ComputeIk(eetrans,eerot,pfree,solutions);
}

IKFAST_API const char* GetKinematicsHash() { return "f5648dba87d158b7a6ee8d512dec18b5"; }

IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }

#ifdef IKFAST_NAMESPACE
} // end namespace
#endif

#ifndef IKFAST_NO_MAIN
#include <stdio.h>
#include <stdlib.h>
#ifdef IKFAST_NAMESPACE
using namespace IKFAST_NAMESPACE;
#endif
int main(int argc, char** argv)
{
    if( argc != 12+GetNumFreeParameters()+1 ) {
        printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
               "Returns the ik solutions given the transformation of the end effector specified by\n"
               "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
               "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
        return 1;
    }

    IkSolutionList<IkReal> solutions;
    std::vector<IkReal> vfree(GetNumFreeParameters());
    IkReal eerot[9],eetrans[3];
    eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
    eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
    eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
    for(std::size_t i = 0; i < vfree.size(); ++i)
        vfree[i] = atof(argv[13+i]);
    bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);

    if( !bSuccess ) {
        fprintf(stderr,"Failed to get ik solution\n");
        return -1;
    }

    printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
    std::vector<IkReal> solvalues(GetNumJoints());
    for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
        const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
        printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
        std::vector<IkReal> vsolfree(sol.GetFree().size());
        sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
        for( std::size_t j = 0; j < solvalues.size(); ++j)
            printf("%.15f, ", solvalues[j]);
        printf("\n");
    }
    return 0;
}

#endif