posemath library is incorrect

Hi,

I was reading posemath library and getting a problem.Seem, there are some wrong functions in this library.

Example, here is roll-pitch-yall rotation:

and here is function "pmMatRpyConvert" in github.com/LinuxCNC/linuxcnc/blob/master...posemath/_posemath.c :

int pmMatRpyConvert(PmRotationMatrix const * const m, PmRpy * const 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) {
rpy->r = atan2(m->y.x, m->y.y);
rpy->p = (2 * PM_PI); /* force it */
rpy->y = 0.0;
} else if (fabs(rpy->p + (2 * PM_PI)) < RPY_P_FUZZ) {
rpy->r = -atan2(m->y.z, m->y.y);
rpy->p = -(2 * PM_PI); /* force it */
rpy->y = 0.0;
} else {
rpy->r = atan2(m->y.z, m->z.z);
rpy->y = atan2(m->x.y, m->x.x);
}

return pmErrno = 0;
}

The condition should be "fabs(rpy->p - (PM_PI/2)) < RPY_P_FUZZ", replaced 2*PM_PI by PM_PI/2

ok, i did

Interesting. I can't remember well, but...
In my custom pentapod kinematics I had to create my own functions where pmMatRpyConvert could be used.
So I probably tried using pmMatRpyConvert and it did not work properly.

i think you cant use pmMatRpyConvert function because you installed your custom kinematics out side source tree, installer completed but when you load module it's always say something like " pmMatRpyConvert undefine"

I included the kinematics into the source tree, and I successfully use other posemath functions, like pmRpyMatConvert.
But if you use halcompile they won't work.