int pmRotRpyConvert(PmRotationVector const * const r, PmRpy * const rpy)
{
PmQuaternion q;
int r1, r2;
q.s = q.x = q.y = q.z = 0.0;
r1 = pmRotQuatConvert(r, &q);
r2 = pmQuatRpyConvert(&q, rpy);
return r1 || r2 ? pmErrno : 0;
}
int kinematicsForward(const double * joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double s1, s2, s3, s4, s5, s6;
double c1, c2, c3, c4, c5, c6;
double s23;
double c23;
double t1, t2, t3, t4, t5;
double sumSq, k;
PmHomogeneous hom;
PmPose worldPose;
PmRpy rpy;
/* Calculate sin of joints for future use */
s1 = sin(joint[0]*PM_PI/180);
s2 = sin(joint[1]*PM_PI/180);
s3 = sin(joint[2]*PM_PI/180);
s4 = sin(joint[3]*PM_PI/180);
s5 = sin(joint[4]*PM_PI/180);
s6 = sin(joint[5]*PM_PI/180);
/* Calculate cos of joints for future use */
c1 = cos(joint[0]*PM_PI/180);
c2 = cos(joint[1]*PM_PI/180);
c3 = cos(joint[2]*PM_PI/180);
c4 = cos(joint[3]*PM_PI/180);
c5 = cos(joint[4]*PM_PI/180);
c6 = cos(joint[5]*PM_PI/180);
s23 = c2 * s3 + s2 * c3;
c23 = c2 * c3 - s2 * s3;
/* Calculate terms to be used in definition of... */
/* first column of rotation matrix. */
t1 = c4 * c5 * c6 - s4 * s6;
t2 = s23 * s5 * c6;
t3 = s4 * c5 * c6 + c4 * s6;
t4 = c23 * t1 - t2;
t5 = c23 * s5 * c6;
/* Define first column of rotation matrix */
hom.rot.x.x = c1 * t4 + s1 * t3;
hom.rot.x.y = s1 * t4 - c1 * t3;
hom.rot.x.z = -s23 * t1 - t5;
/* Calculate terms to be used in definition of... */
/* second column of rotation matrix. */
t1 = -c4 * c5 * s6 - s4 * c6;
t2 = s23 * s5 * s6;
t3 = c4 * c6 - s4 * c5 * s6;
t4 = c23 * t1 + t2;
t5 = c23 * s5 * s6;
/* Define second column of rotation matrix */
hom.rot.y.x = c1 * t4 + s1 * t3;
hom.rot.y.y = s1 * t4 - c1 * t3;
hom.rot.y.z = -s23 * t1 + t5;
/* Calculate term to be used in definition of... */
/* third column of rotation matrix. */
t1 = c23 * c4 * s5 + s23 * c5;
/* Define third column of rotation matrix */
hom.rot.z.x = -c1 * t1 - s1 * s4 * s5;
hom.rot.z.y = -s1 * t1 + c1 * s4 * s5;
hom.rot.z.z = s23 * c4 * s5 - c23 * c5;
/* Calculate term to be used in definition of... */
/* position vector. */
t1 = PUMA_A2 * c2 + PUMA_A3 * c23 - PUMA_D4 * s23;
/* Define position vector */
hom.tran.x = c1 * t1 - PUMA_D3 * s1;
hom.tran.y = s1 * t1 + PUMA_D3 * c1;
hom.tran.z = -PUMA_A3 * s23 - PUMA_A2 * s2 - PUMA_D4 * c23;
/* Calculate terms to be used to... */
/* determine flags. */
sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
PUMA_D3 * PUMA_D3;
k = (sumSq + hom.tran.z * hom.tran.z - PUMA_A2 * PUMA_A2 -
PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) /
(2.0 * PUMA_A2);
/* reset flags */
*iflags = 0;
/* Set shoulder-up flag if necessary */
if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) +
atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ)
{
*iflags |= PUMA_SHOULDER_RIGHT;
}
/* Set elbow down flag if necessary */
if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) +
atan2(k, -sqrt(PUMA_A3 * PUMA_A3 +
PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ)
{
*iflags |= PUMA_ELBOW_DOWN;
}
/* set singular flag if necessary */
t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1;
t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 +
hom.rot.z.z * s23;
if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ)
{
*iflags |= PUMA_SINGULAR;
}
/* if not singular set wrist flip flag if necessary */
else{
if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ))
{
*iflags |= PUMA_WRIST_FLIP;
}
}
/* convert hom.rot to world->quat */
pmHomPoseConvert(hom, &worldPose);
pmQuatRpyConvert(worldPose.rot,&rpy);
world->tran = worldPose.tran;
world->a = rpy.r * 180.0/PM_PI;
world->b = rpy.p * 180.0/PM_PI;
world->c = rpy.y * 180.0/PM_PI;
/* return 0 and exit */
return 0;
}
