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