int pmRpyRotConvert(PmRpy const * const rpy, PmRotationVector * const r)
{
PmQuaternion q;
int r1, r2;
q.s = q.x = q.y = q.z = 0.0;
r->s = r->x = r->y = r->z = 0.0;
r1 = pmRpyQuatConvert(rpy, &q);
r2 = pmQuatRotConvert(&q, r);
return r1 || r2 ? pmErrno : 0;
}
int kinematicsInverse(const EmcPose * world,
double * joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmHomogeneous hom;
PmPose worldPose;
PmRpy rpy;
int singular;
double t1, t2, t3;
double k;
double sumSq;
double th1;
double th3;
double th23;
double th2;
double th4;
double th5;
double th6;
double s1, c1;
double s3, c3;
double s23, c23;
double s4, c4;
double s5, c5;
double s6, c6;
/* reset flags */
*fflags = 0;
/* convert pose to hom */
worldPose.tran = world->tran;
rpy.r = world->a*PM_PI/180.0;
rpy.p = world->b*PM_PI/180.0;
rpy.y = world->c*PM_PI/180.0;
pmRpyQuatConvert(rpy,&worldPose.rot);
pmPoseHomConvert(worldPose, &hom);
/* Joint 1 (2 independent solutions) */
/* save sum of squares for this and subsequent calcs */
sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
PUMA_D3 * PUMA_D3;
/* FIXME-- is use of + sqrt shoulder right or left? */
if (*iflags & PUMA_SHOULDER_RIGHT){
th1 = atan2(hom.tran.y, hom.tran.x) - atan2(PUMA_D3, -sqrt(sumSq));
}
else{
th1 = atan2(hom.tran.y, hom.tran.x) - atan2(PUMA_D3, sqrt(sumSq));
}
/* save sin, cos for later calcs */
s1 = sin(th1);
c1 = cos(th1);
/* Joint 3 (2 independent solutions) */
k = (sumSq + hom.tran.z * hom.tran.z - PUMA_A2 * PUMA_A2 -
PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) / (2.0 * PUMA_A2);
/* FIXME-- is use of + sqrt elbow up or down? */
if (*iflags & PUMA_ELBOW_DOWN){
th3 = atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k));
}
else{
th3 = atan2(PUMA_A3, PUMA_D4) -
atan2(k, sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k));
}
/* compute sin, cos for later calcs */
s3 = sin(th3);
c3 = cos(th3);
/* Joint 2 */
t1 = (-PUMA_A3 - PUMA_A2 * c3) * hom.tran.z +
(c1 * hom.tran.x + s1 * hom.tran.y) * (PUMA_A2 * s3 - PUMA_D4);
t2 = (PUMA_A2 * s3 - PUMA_D4) * hom.tran.z +
(PUMA_A3 + PUMA_A2 * c3) * (c1 * hom.tran.x + s1 * hom.tran.y);
t3 = hom.tran.z * hom.tran.z + (c1 * hom.tran.x + s1 * hom.tran.y) *
(c1 * hom.tran.x + s1 * hom.tran.y);
th23 = atan2(t1, t2);
th2 = th23 - th3;
/* compute sin, cos for later calcs */
s23 = t1 / t3;
c23 = t2 / t3;
/* Joint 4 */
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){
singular = 1;
*fflags |= PUMA_REACH;
th4 = joint[3]*PM_PI/180; /* use current value */
}
else{
singular = 0;
th4 = atan2(t1, t2);
}
/* compute sin, cos for later calcs */
s4 = sin(th4);
c4 = cos(th4);
/* Joint 5 */
s5 = hom.rot.z.z * (s23 * c4) -
hom.rot.z.x * (c1 * c23 * c4 + s1 * s4) -
hom.rot.z.y * (s1 * c23 * c4 - c1 * s4);
c5 =-hom.rot.z.x * (c1 * s23) - hom.rot.z.y *
(s1 * s23) - hom.rot.z.z * c23;
th5 = atan2(s5, c5);
/* Joint 6 */
s6 = hom.rot.x.z * (s23 * s4) - hom.rot.x.x *
(c1 * c23 * s4 - s1 * c4) - hom.rot.x.y *
(s1 * c23 * s4 + c1 * c4);
c6 = hom.rot.x.x * ((c1 * c23 * c4 + s1 * s4) *
c5 - c1 * s23 * s5) + hom.rot.x.y *
((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) -
hom.rot.x.z * (s23 * c4 * c5 + c23 * s5);
th6 = atan2(s6, c6);
/* FIXME-- is wrist flip the normal or offset results? */
if (*iflags & PUMA_WRIST_FLIP){
th4 = th4 + PM_PI;
th5 = -th5;
th6 = th6 + PM_PI;
}
/* copy out */
joint[0] = th1*180/PM_PI;
joint[1] = th2*180/PM_PI;
joint[2] = th3*180/PM_PI;
joint[3] = th4*180/PM_PI;
joint[4] = th5*180/PM_PI;
joint[5] = th6*180/PM_PI;
return singular == 0 ? 0 : -1;
}
