the study of arithmetic

We want to use PUMA560 with EMC2-2.4.5,so we study the arithmetic about industry robot.
But we come upon a problem in file _posemath.c at functiong "pmMatRpyConvert".
The function in this file list as bellow:

int pmMatRpyConvert(PmRotationMatrix m, PmRpy * rpy)
{
rpy->p = atan2(-m.x.z, pmSqrt(pmSq(m.x.x) + pmSq(m.x.y)));
if (fabs(rpy->p - (2 * PM_PI)) < RPY_P_FUZZ) {
......
} else if (fabs(rpy->p + (2 * PM_PI)) < RPY_P_FUZZ) {
......
} else {
......
}
return pmErrno = 0;
}
we known atan2 is redifined in file
Rtapi_math_i386.h
"
extern __inline double atan2 (double __y, double __x) {
register long double __value;
__asm __volatile__ ("fpatan" : "=t" (__value) : "0" (__x), "u" (__y) : "st(1)");
return __value;
}
"
But the value range of "atan2" and "fpatan" are (-PM_PI,PM_PI),
so the value range of "rpy->p" also is (-PM_PI,PM_PI),
then how can the programme run into the subarea of
"
if (fabs(rpy->p - (2 * PM_PI)) < RPY_P_FUZZ) {
......
}
"
also as
"
else if (fabs(rpy->p + (2 * PM_PI)) < RPY_P_FUZZ) {
......
}
"
thanks Fred Proctor.
thanks Will Shackleford.
thanks for your time.

The intent of the tests in both this function in _posemath.c and the preceding one, pmMatZyxConvert(), is to deal with edge cases. It would appear you are right that both incorrectly use +/-2 * PM_PI as the limits, so the edge-case code isn't exercised.

The posemath code can be traced back to early days of the development of the Realtime Control System (RCS) at NIST. As you can see, the functions are not well documented.

I don't want to propose a fix until I'm sure of my facts, but thanks for raising the question...and of course you can propose fixes too.

Regards,
Kent


*not necessarily either Fred or Will; they incorporated this code in EMC (which became LinuxCNC) but lots of folks contributed to RCS over the years.