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; }