double y0(double x)
{
double z,s,c,ss,cc,u,v;
int32_t hx,ix,lx;
EXTRACT_WORDS(hx, lx, x);
ix = 0x7fffffff & hx;
/* Y0(NaN) is NaN, y0(-inf) is Nan, y0(inf) is 0 */
if (ix >= 0x7ff00000)
return 1.0/(x+x*x);
if ((ix|lx) == 0)
return -1.0/0.0;
if (hx < 0)
return 0.0/0.0;
if (ix >= 0x40000000) { /* |x| >= 2.0 */
/* y0(x) = sqrt(2/(pi*x))*(p0(x)*sin(x0)+q0(x)*cos(x0))
* where x0 = x-pi/4
* Better formula:
* cos(x0) = cos(x)cos(pi/4)+sin(x)sin(pi/4)
* = 1/sqrt(2) * (sin(x) + cos(x))
* sin(x0) = sin(x)cos(3pi/4)-cos(x)sin(3pi/4)
* = 1/sqrt(2) * (sin(x) - cos(x))
* To avoid cancellation, use
* sin(x) +- cos(x) = -cos(2x)/(sin(x) -+ cos(x))
* to compute the worse one.
*/
s = sin(x);
c = cos(x);
ss = s-c;
cc = s+c;
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if (ix < 0x7fe00000) { /* make sure x+x does not overflow */
z = -cos(x+x);
if (s*c < 0.0)
cc = z/ss;
else
ss = z/cc;
}
if (ix > 0x48000000)
z = (invsqrtpi*ss)/sqrt(x);
else {
u = pzero(x);
v = qzero(x);
z = invsqrtpi*(u*ss+v*cc)/sqrt(x);
}
return z;
}
if (ix <= 0x3e400000) { /* x < 2**-27 */
return u00 + tpi*log(x);
}
z = x*x;
u = u00+z*(u01+z*(u02+z*(u03+z*(u04+z*(u05+z*u06)))));
v = 1.0+z*(v01+z*(v02+z*(v03+z*v04)));
return u/v + tpi*(j0(x)*log(x));
}
double
__ieee754_j0(double x)
{
double z, s,c,ss,cc,r,u,v,r1,r2,s1,s2,z2,z4;
int32_t hx,ix;
GET_HIGH_WORD(hx,x);
ix = hx&0x7fffffff;
if(ix>=0x7ff00000) return one/(x*x);
x = fabs(x);
if(ix >= 0x40000000) { /* |x| >= 2.0 */
__sincos (x, &s, &c);
ss = s-c;
cc = s+c;
if(ix<0x7fe00000) { /* make sure x+x not overflow */
z = -__cos(x+x);
if ((s*c)<zero) cc = z/ss;
else ss = z/cc;
}
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if(ix>0x48000000) z = (invsqrtpi*cc)/__ieee754_sqrt(x);
else {
u = pzero(x); v = qzero(x);
z = invsqrtpi*(u*cc-v*ss)/__ieee754_sqrt(x);
}
return z;
}
if(ix<0x3f200000) { /* |x| < 2**-13 */
math_force_eval(huge+x); /* raise inexact if x != 0 */
if(ix<0x3e400000) return one; /* |x|<2**-27 */
else return one - 0.25*x*x;
}
z = x*x;
#ifdef DO_NOT_USE_THIS
r = z*(R02+z*(R03+z*(R04+z*R05)));
s = one+z*(S01+z*(S02+z*(S03+z*S04)));
#else
r1 = z*R[2]; z2=z*z;
r2 = R[3]+z*R[4]; z4=z2*z2;
r = r1 + z2*r2 + z4*R[5];
s1 = one+z*S[1];
s2 = S[2]+z*S[3];
s = s1 + z2*s2 + z4*S[4];
#endif
if(ix < 0x3FF00000) { /* |x| < 1.00 */
return one + z*(-0.25+(r/s));
} else {
u = 0.5*x;
return((one+u)*(one-u)+z*(r/s));
}
}
double
j0 (double x)
{
if (x <= 0.0)
x = -x;
if (x < 8.0)
return jzero(x);
else
return (sqrt(TWO_ON_PI/x)
* (pzero(x) * cos (x - PI_ON_FOUR)
- 8.0/x * qzero(x) * sin (x - PI_ON_FOUR)));
}
double j0(double x)
{
double z, s,c,ss,cc,r,u,v;
int32_t hx,ix;
GET_HIGH_WORD(hx, x);
ix = hx & 0x7fffffff;
if (ix >= 0x7ff00000)
return 1.0/(x*x);
x = fabs(x);
if (ix >= 0x40000000) { /* |x| >= 2.0 */
s = sin(x);
c = cos(x);
ss = s-c;
cc = s+c;
if (ix < 0x7fe00000) { /* make sure x+x does not overflow */
z = -cos(x+x);
if (s*c < 0.0)
cc = z/ss;
else
ss = z/cc;
}
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if (ix > 0x48000000)
z = (invsqrtpi*cc)/sqrt(x);
else {
u = pzero(x);
v = qzero(x);
z = invsqrtpi*(u*cc-v*ss)/sqrt(x);
}
return z;
}
if (ix < 0x3f200000) { /* |x| < 2**-13 */
/* raise inexact if x != 0 */
if (huge+x > 1.0) {
if (ix < 0x3e400000) /* |x| < 2**-27 */
return 1.0;
return 1.0 - 0.25*x*x;
}
}
z = x*x;
r = z*(R02+z*(R03+z*(R04+z*R05)));
s = 1.0+z*(S01+z*(S02+z*(S03+z*S04)));
if (ix < 0x3FF00000) { /* |x| < 1.00 */
return 1.0 + z*(-0.25+(r/s));
} else {
u = 0.5*x;
return (1.0+u)*(1.0-u) + z*(r/s);
}
}
double __ieee754_y0(double x)
{
double z, s,c,ss,cc,u,v;
int hx,ix,lx;
hx = CYG_LIBM_HI(x);
ix = 0x7fffffff&hx;
lx = CYG_LIBM_LO(x);
/* Y0(NaN) is NaN, y0(-inf) is Nan, y0(inf) is 0 */
if(ix>=0x7ff00000) return one/(x+x*x);
if((ix|lx)==0) return -one/zero;
if(hx<0) return zero/zero;
if(ix >= 0x40000000) { /* |x| >= 2.0 */
/* y0(x) = sqrt(2/(pi*x))*(p0(x)*sin(x0)+q0(x)*cos(x0))
* where x0 = x-pi/4
* Better formula:
* cos(x0) = cos(x)cos(pi/4)+sin(x)sin(pi/4)
* = 1/sqrt(2) * (sin(x) + cos(x))
* sin(x0) = sin(x)cos(3pi/4)-cos(x)sin(3pi/4)
* = 1/sqrt(2) * (sin(x) - cos(x))
* To avoid cancellation, use
* sin(x) +- cos(x) = -cos(2x)/(sin(x) -+ cos(x))
* to compute the worse one.
*/
s = sin(x);
c = cos(x);
ss = s-c;
cc = s+c;
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if(ix<0x7fe00000) { /* make sure x+x not overflow */
z = -cos(x+x);
if ((s*c)<zero) cc = z/ss;
else ss = z/cc;
}
if(ix>0x48000000) z = (invsqrtpi*ss)/sqrt(x);
else {
u = pzero(x); v = qzero(x);
z = invsqrtpi*(u*ss+v*cc)/sqrt(x);
}
return z;
}
if(ix<=0x3e400000) { /* x < 2**-27 */
return(u00 + tpi*__ieee754_log(x));
}
z = x*x;
u = u00+z*(u01+z*(u02+z*(u03+z*(u04+z*(u05+z*u06)))));
v = one+z*(v01+z*(v02+z*(v03+z*v04)));
return(u/v + tpi*(__ieee754_j0(x)*__ieee754_log(x)));
}
double fd_y0(double x) /* wrapper fd_y0 */
{
double z, s,c,ss,cc,u,v;
int hx,ix,lx;
hx = FD_HI(x);
ix = 0x7fffffff&hx;
lx = FD_LO(x);
/* Y0(NaN) is NaN, fd_y0(-inf) is Nan, fd_y0(inf) is 0 */
if(ix>=0x7ff00000) return gD( one, gA(x, gM(x,x)));
if((ix|lx)==0) return gD(-one, zero);
if(hx<0) return gD(zero,zero);
if(ix >= 0x40000000) { /* |x| >= 2.0 */
/* fd_y0(x) = fd_sqrt(2/(pi*x))*(p0(x)*fd_sin(x0)+q0(x)*fd_cos(x0))
* where x0 = x-pi/4
* Better formula:
* fd_cos(x0) = fd_cos(x)fd_cos(pi/4)+fd_sin(x)fd_sin(pi/4)
* = 1/fd_sqrt(2) * (fd_sin(x) + fd_cos(x))
* fd_sin(x0) = fd_sin(x)fd_cos(3pi/4)-fd_cos(x)fd_sin(3pi/4)
* = 1/fd_sqrt(2) * (fd_sin(x) - fd_cos(x))
* To avoid cancellation, use
* fd_sin(x) +- fd_cos(x) = -fd_cos(2x)/(fd_sin(x) -+ fd_cos(x))
* to compute the worse one.
*/
s = fd_sin(x);
c = fd_cos(x);
ss = gS(s,c);
cc = gA(s,c);
/*
* fd_j0(x) = 1/fd_sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / fd_sqrt(x)
* fd_y0(x) = 1/fd_sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / fd_sqrt(x)
*/
if(ix<0x7fe00000) { /* make sure x+x not overflow */
z = -fd_cos(gA(x,x));
if (gM(s,c) < zero) cc = gD(z,ss);
else ss = gD(z,cc);
}
if(ix>0x48000000) z = gD(gM(invsqrtpi,ss), gSqrt(x));
else {
u = pzero(x); v = qzero(x);
z = gD(gM(invsqrtpi, gA(gM(u,ss), gM(v,cc))), gSqrt(x));
}
return z;
}
if(ix<=0x3e400000) { /* x < 2**-27 */
return gA(u00, gM(tpi, fd_log(x)));
}
z = gM(x,x);
u = gA(u00, gM(z,gA(u01, gM(z,gA(u02, gM(z,gA(u03, gM(z,gA(u04, gM(z,gA(u05, gM(z,u06))))))))))));
v = gA(one, gM(z,gA(v01, gM(z,gA(v02, gM(z,gA(v03, gM(z,v04))))))));
return(gD(u,v) + gM(tpi,gM(fd_j0(x), fd_log(x))));
}
double
y0 (double x)
{
if (x < 0.0)
return (dzero/dzero); /* IEEE machines: invalid operation */
if (x < 8.0)
return yzero(x) + TWO_ON_PI * j0(x) * log(x);
else
return (sqrt (TWO_ON_PI/x)
* (pzero(x) * sin (x - PI_ON_FOUR)
+ (8.0/x) * qzero(x) * cos(x - PI_ON_FOUR)));
}
double __ieee754_j0(double x)
{
double z, s,c,ss,cc,r,u,v;
int hx,ix;
hx = CYG_LIBM_HI(x);
ix = hx&0x7fffffff;
if(ix>=0x7ff00000) return one/(x*x);
x = fabs(x);
if(ix >= 0x40000000) { /* |x| >= 2.0 */
s = sin(x);
c = cos(x);
ss = s-c;
cc = s+c;
if(ix<0x7fe00000) { /* make sure x+x not overflow */
z = -cos(x+x);
if ((s*c)<zero) cc = z/ss;
else ss = z/cc;
}
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if(ix>0x48000000) z = (invsqrtpi*cc)/sqrt(x);
else {
u = pzero(x); v = qzero(x);
z = invsqrtpi*(u*cc-v*ss)/sqrt(x);
}
return z;
}
if(ix<0x3f200000) { /* |x| < 2**-13 */
if(huge+x>one) { /* raise inexact if x != 0 */
if(ix<0x3e400000) return one; /* |x|<2**-27 */
else return one - 0.25*x*x;
}
}
z = x*x;
r = z*(R02+z*(R03+z*(R04+z*R05)));
s = one+z*(S01+z*(S02+z*(S03+z*S04)));
if(ix < 0x3FF00000) { /* |x| < 1.00 */
return one + z*(-0.25+(r/s));
} else {
u = 0.5*x;
return((one+u)*(one-u)+z*(r/s));
}
}
double fd_j0(double x) /* wrapper fd_j0 */
{
double z, s,c,ss,cc,r,u,v;
int hx,ix;
hx = FD_HI(x);
ix = hx&0x7fffffff;
if(ix>=0x7ff00000) return gD(one, gM(x,x));
x = fd_fabs(x);
if(ix >= 0x40000000) { /* |x| >= 2.0 */
s = fd_sin(x);
c = fd_cos(x);
ss = gS(s,c);
cc = gA(s,c);
if(ix<0x7fe00000) { /* make sure x+x not overflow */
z = -fd_cos(gA(x,x));
if (gM(s,c) < zero) cc = gD(z,ss);
else ss = gD(z,cc);
}
/*
* fd_j0(x) = 1/fd_sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / fd_sqrt(x)
* fd_y0(x) = 1/fd_sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / fd_sqrt(x)
*/
if(ix>0x48000000) z = gD(gM(invsqrtpi,cc),gSqrt(x));
else {
u = pzero(x); v = qzero(x);
z = gD(gM(invsqrtpi, (gS(gM(u,cc), gM(v,ss)))), gSqrt(x));
}
return z;
}
if(ix<0x3f200000) { /* |x| < 2**-13 */
if(huge+x>one) { /* raise inexact if x != 0 */
if(ix<0x3e400000) return one; /* |x|<2**-27 */
else return gS(one, gM(gM(0.25,x),x));
}
}
z = gM(x,x);
r = gM(z,gA(R02, gM(z,gA(R03, gM(z,gA(R04, gM(z,R05)))))));
s = gA(one, gM(z,gA(S01, gM(z,gA(S02, gM(z,gA(S03, gM(z,S04))))))));
if(ix < 0x3FF00000) { /* |x| < 1.00 */
return gA(one, gM(z,gA(-0.25,gD(r,s))));
} else {
u = gM(0.5,x);
return(gM(gA(one, u), gS(one, u)) + gM(z, gD(r,s)));
}
}
double
__ieee754_y0(double x)
{
double z, s,c,ss,cc,u,v,z2,z4,z6,u1,u2,u3,v1,v2;
int32_t hx,ix,lx;
EXTRACT_WORDS(hx,lx,x);
ix = 0x7fffffff&hx;
/* Y0(NaN) is NaN, y0(-inf) is Nan, y0(inf) is 0, y0(0) is -inf. */
if(ix>=0x7ff00000) return one/(x+x*x);
if((ix|lx)==0) return -HUGE_VAL+x; /* -inf and overflow exception. */
if(hx<0) return zero/(zero*x);
if(ix >= 0x40000000) { /* |x| >= 2.0 */
/* y0(x) = sqrt(2/(pi*x))*(p0(x)*sin(x0)+q0(x)*cos(x0))
* where x0 = x-pi/4
* Better formula:
* cos(x0) = cos(x)cos(pi/4)+sin(x)sin(pi/4)
* = 1/sqrt(2) * (sin(x) + cos(x))
* sin(x0) = sin(x)cos(3pi/4)-cos(x)sin(3pi/4)
* = 1/sqrt(2) * (sin(x) - cos(x))
* To avoid cancellation, use
* sin(x) +- cos(x) = -cos(2x)/(sin(x) -+ cos(x))
* to compute the worse one.
*/
__sincos (x, &s, &c);
ss = s-c;
cc = s+c;
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if(ix<0x7fe00000) { /* make sure x+x not overflow */
z = -__cos(x+x);
if ((s*c)<zero) cc = z/ss;
else ss = z/cc;
}
if(ix>0x48000000) z = (invsqrtpi*ss)/__ieee754_sqrt(x);
else {
u = pzero(x); v = qzero(x);
z = invsqrtpi*(u*ss+v*cc)/__ieee754_sqrt(x);
}
return z;
}
if(ix<=0x3e400000) { /* x < 2**-27 */
return(U[0] + tpi*__ieee754_log(x));
}
z = x*x;
#ifdef DO_NOT_USE_THIS
u = u00+z*(u01+z*(u02+z*(u03+z*(u04+z*(u05+z*u06)))));
v = one+z*(v01+z*(v02+z*(v03+z*v04)));
#else
u1 = U[0]+z*U[1]; z2=z*z;
u2 = U[2]+z*U[3]; z4=z2*z2;
u3 = U[4]+z*U[5]; z6=z4*z2;
u = u1 + z2*u2 + z4*u3 + z6*U[6];
v1 = one+z*V[0];
v2 = V[1]+z*V[2];
v = v1 + z2*v2 + z4*V[3];
#endif
return(u/v + tpi*(__ieee754_j0(x)*__ieee754_log(x)));
}
GENERIC
j0(GENERIC x) {
GENERIC z, s, c, ss, cc, r, u, v, ox;
int i;
if (isnan(x))
return (x*x); /* + -> * for Cheetah */
ox = x;
x = fabs(x);
if (x > 8.0) {
if (!finite(x))
return (zero);
s = sin(x);
c = cos(x);
/*
* j0(x) = sqrt(2/(pi*x))*(p0(x)*cos(x0)-q0(x)*sin(x0))
* where x0 = x-pi/4
* Better formula:
* cos(x0) = cos(x)cos(pi/4)+sin(x)sin(pi/4)
* = 1/sqrt(2) * (cos(x) + sin(x))
* sin(x0) = sin(x)cos(pi/4)-cos(x)sin(pi/4)
* = 1/sqrt(2) * (sin(x) - cos(x))
* To avoid cancellation, use
* sin(x) +- cos(x) = -cos(2x)/(sin(x) -+ cos(x))
* to compute the worse one.
*/
if (x > 8.9e307) { /* x+x may overflow */
ss = s-c;
cc = s+c;
} else if (signbit(s) != signbit(c)) {
ss = s - c;
cc = -cos(x+x)/ss;
} else {
cc = s + c;
ss = -cos(x+x)/cc;
}
/*
* j0(x) = 1/sqrt(pi) * (P(0,x)*cc - Q(0,x)*ss) / sqrt(x)
* y0(x) = 1/sqrt(pi) * (P(0,x)*ss + Q(0,x)*cc) / sqrt(x)
*/
if (x > 1.0e40) z = (invsqrtpi*cc)/sqrt(x);
else {
u = pzero(x); v = qzero(x);
z = invsqrtpi*(u*cc-v*ss)/sqrt(x);
}
/* force to pass SVR4 even the result is wrong (sign) */
if (x > X_TLOSS)
return (_SVID_libm_err(ox, z, 34));
else
return (z);
}
if (x <= small) {
if (x <= tiny)
return (one-x);
else
return (one-x*x*0.25);
}
z = x*x;
if (x <= 1.28) {
r = r0[0]+z*(r0[1]+z*(r0[2]+z*r0[3]));
s = s0[0]+z*(s0[1]+z*(s0[2]+z*s0[3]));
return (one + z*(r/s));
} else {
for (r = r1[8], s = s1[8], i = 7; i >= 0; i--) {
r = r*z + r1[i];
s = s*z + s1[i];
}
return (r/s);
}
}
GENERIC
y0(GENERIC x) {
GENERIC z, /* d, */ s, c, ss, cc, u, v;
int i;
if (isnan(x))
return (x*x); /* + -> * for Cheetah */
if (x <= zero) {
if (x == zero)
/* d= -one/(x-x); */
return (_SVID_libm_err(x, x, 8));
else
/* d = zero/(x-x); */
return (_SVID_libm_err(x, x, 9));
}
if (x > 8.0) {
if (!finite(x))
return (zero);
s = sin(x);
c = cos(x);
/*
* j0(x) = sqrt(2/(pi*x))*(p0(x)*cos(x0)-q0(x)*sin(x0))
* where x0 = x-pi/4
* Better formula:
* cos(x0) = cos(x)cos(pi/4)+sin(x)sin(pi/4)
* = 1/sqrt(2) * (cos(x) + sin(x))
* sin(x0) = sin(x)cos(pi/4)-cos(x)sin(pi/4)
* = 1/sqrt(2) * (sin(x) - cos(x))
* To avoid cancellation, use
* sin(x) +- cos(x) = -cos(2x)/(sin(x) -+ cos(x))
* to compute the worse one.
*/
if (x > 8.9e307) { /* x+x may overflow */
ss = s-c;
cc = s+c;
} else if (signbit(s) != signbit(c)) {
ss = s - c;
cc = -cos(x+x)/ss;
} else {
cc = s + c;
ss = -cos(x+x)/cc;
}
/*
* j0(x) = 1/sqrt(pi*x) * (P(0,x)*cc - Q(0,x)*ss)
* y0(x) = 1/sqrt(pi*x) * (P(0,x)*ss + Q(0,x)*cc)
*/
if (x > 1.0e40)
z = (invsqrtpi*ss)/sqrt(x);
else
z = invsqrtpi*(pzero(x)*ss+qzero(x)*cc)/sqrt(x);
if (x > X_TLOSS)
return (_SVID_libm_err(x, z, 35));
else
return (z);
}
if (x <= tiny) {
return (u0[0] + tpi*log(x));
}
z = x*x;
for (u = u0[12], i = 11; i >= 0; i--) u = u*z + u0[i];
v = v0[0]+z*(v0[1]+z*(v0[2]+z*(v0[3]+z*v0[4])));
return (u/v + tpi*(j0(x)*log(x)));
}
int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlInverseKinematicsTest KINEMATICSFILE" << std::endl;
return 1;
}
try
{
std::srand(0); // get reproducible results
rl::kin::Kinematics* kinematics = static_cast< ::rl::kin::Kinematics* >(rl::kin::Kinematics::create(argv[1]));
std::size_t nTests;
rl::math::Vector q(6);
rl::math::Vector qinv(6);
rl::math::Vector qzero(6);
std::size_t n;
std::size_t wrongs;
std::size_t wrongT;
std::size_t ngotinverse;
for (std::size_t ispuma = 0; ispuma < 2; ++ispuma)
{
nTests = 0 == ispuma ? 1000 : 100;
for (n = 0, wrongs = 0, wrongT = 0, ngotinverse = 0; n < nTests && wrongT < 100 && wrongs < 100; ++n)
{
for (std::size_t i = 0; i < 6; ++i)
{
rl::math::Real r = static_cast< rl::math::Real >(std::rand()) / static_cast< rl::math::Real >(RAND_MAX);
q(i) = (r - 0.5) * 360 * rl::math::DEG2RAD;
qzero(i) = 0;
}
kinematics->setPosition(q);
kinematics->updateFrames();
rl::math::Transform t = kinematics->forwardPosition();
// For iterative inverse, set starting point far away
kinematics->setPosition(qzero);
kinematics->updateFrames();
if (0 == ispuma)
{
rl::kin::Puma::Arm arm;
rl::kin::Puma::Elbow elbow;
rl::kin::Puma::Wrist wrist;
dynamic_cast< rl::kin::Puma* >(kinematics)->parameters(q, arm, elbow, wrist);
dynamic_cast< rl::kin::Puma* >(kinematics)->setArm(arm);
dynamic_cast< rl::kin::Puma* >(kinematics)->setElbow(elbow);
dynamic_cast< rl::kin::Puma* >(kinematics)->setWrist(wrist);
}
if (
0 == ispuma
? dynamic_cast< ::rl::kin::Puma* >(kinematics)->inversePosition(t, qinv)
: dynamic_cast< ::rl::kin::Kinematics* >(kinematics)->inversePosition(t, qinv, 0, 100)
)
{
kinematics->setPosition(qinv);
kinematics->updateFrames();
rl::math::Transform tinv = kinematics->forwardPosition();
if ((t.matrix() - tinv.matrix()).norm() > 1e-6)
{
++wrongT;
}
if ((q - qinv).norm() > 1e-4)
{
++wrongs;
}
if (wrongT < 3 && (t.matrix() - tinv.matrix()).norm() > 1e-6)
{
std::cout << " q = " << q.transpose() << std::endl;
std::cout << " T = " << t.matrix() << std::endl;
std::cout << " qinv = " << qinv.transpose() << std::endl;
std::cout << "Wrong Tinv = " << tinv.matrix() << std::endl;
std::cout << std::endl;
}
++ngotinverse;
}
}
std::cout << "Notice: "
<< (0 == ispuma ? "Puma direct " : "Iterative ") << "inverse kinematics "
<< "on file " << argv[1] << " "
<< "tested with " << n << " cases, "
<< ngotinverse << " returned a solution, "
<< "thereof " << wrongs << " in wrong configuration, and "
<< wrongT << " with completely wrong pose."
<< std::endl;
if (wrongT > 0)
{
std::cerr << "Error: "
<< (0 == ispuma ? "Puma direct " : "Iterative ")
<< "inverse kinematics " << "on file " << argv[1]
<< " gave incorrect poses." << std::endl;
return 1;
}
if (0 == ngotinverse)
{
std::cerr << "Error: "
<< (0 == ispuma ? "Puma direct " : "Iterative ")
<< "inverse kinematics "<< "on file " << argv[1]
<< " gave no solutions."
<< std::endl;
return 1;
}
}
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
return 1;
}
return 0;
}
