/** \file * Direct inverse kinesmatics for the PUMA. * * Based on "A geometric approach in solving the inverse kinematics of * PUMA robots" by Lee and Ziegler, 1983. * * http://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf?sequence=5 */ #include #include #include #include "ik.h" int ik_first( double * const theta, // output commands const double * const xyz, // desired position const int right, // right arm = +1, left arm = -1 const int above // elbow above arm = +1, below arm = -1 ) { const double px = xyz[0]; const double py = xyz[1]; const double pz = xyz[2]; const double d2 = 130; // mm from center of rotation to center of arm const double a2 = 205; // mm along first arm const double d4 = 225; // mm along second arm to center of wrist const double a3 = 0; // mm between center of rotation of elbow? if (px*px + py*py < d2*d2) return 0; const double r = sqrt(px*px + py*py - d2*d2); const double R = sqrt(px*px + py*py + pz*pz - d2*d2); //printf("r=%f R=%f\n", r, R); // theta[0] defined in equation 26 theta[0] = atan2( -right * py * r - px * d2, -right * px * r + py * d2 ); // theta[1] is equations 28 - 35. { const double sin_alpha = -pz / R; const double cos_alpha = -right * r / R; const double cos_beta = (a2*a2 + R*R - (d4*d4 + a3*a3)) / (2*a2*R); if (cos_beta > 1 || cos_beta < -1) return 0; const double sin_beta = sqrt(1 - cos_beta*cos_beta); const double sin_t2 = sin_alpha * cos_beta + right * above * cos_alpha * sin_beta; const double cos_t2 = cos_alpha * cos_beta - right * above * sin_alpha * sin_beta; theta[1] = atan2(sin_t2, cos_t2); } // theta[2] { const double t2 = d4*d4 + a3*a3; const double t = sqrt(t2); const double cos_phi = (a2*a2 + t2 - R*R) / (2 * a2 * t); if (cos_phi > 1 || cos_phi < -1) return 0; const double sin_phi = right * above * sqrt(1 - cos_phi*cos_phi); const double sin_beta = d4 / t; const double cos_beta = fabs(a3) / t; const double sin_t3 = sin_phi*cos_beta - cos_phi*sin_beta; const double cos_t3 = cos_phi*cos_beta + sin_phi*sin_beta; theta[2] = atan2(sin_t3, cos_t3); } return 1; } int ik_wrist( double * const theta, // input/output commands const double * const xyz, // desired position (in mm) const double * const a, // desired approach vector const double * const s, // desired sliding vector (for hand opening) const double * const n, // desired normal vector const int wrist // wrist up = +1, wrist down = -1 ) { const double M = -1; const double ax = a[0]; const double ay = a[1]; const double az = a[2]; const double sx = s[0]; const double sy = s[1]; const double sz = s[2]; const double nx = n[0]; const double ny = n[1]; const double nz = n[2]; const double C1 = cos(theta[0]); const double S1 = sin(theta[0]); const double C23 = cos(theta[1] + theta[2]); const double S23 = sin(theta[1] + theta[2]); const double S4 = M * (C1*ay - S1*ax); const double C4 = M * (C1*C23*ax + S1*C23*ay- S23*az); theta[3] = atan2(S4, C4); const double S5 = (C1*C23*C4 - S1*S4)*ax + (S1*C23*C4 + C1*S4)*ay - C4* S23*az; const double C5 = C1*S23*ax + S1*S23*ay + C23*az; theta[4] = atan2(S5,C5); const double S6 = (-S1*C4 - C1*C23*S4)*nx + (C1*C4-S1*C23*S4)*ny + S4*S23*nz; const double C6 = (-S1*C4-C1*C23*S4)*sx + (C1*C4-S1*C23*S4)*sy + S4*S23*sz; theta[5] = atan2(S6,C6); return 1; } #if 0 int main(int argc, char **argv) { if (argc != 4) { fprintf(stderr, "need xyz args\n"); return -1; } const double xyz[3] = { atof(argv[1]), atof(argv[2]), atof(argv[3]) }; double theta[6]; double a[] = { 0, 1, 0 }; double s[] = { 1, 0, 0 }; double n[] = { 0, 0, 1 }; // right, above if (!ik_first(theta, xyz, 1, 1)) { fprintf(stderr, "[%f,%f,%f] unreachable\n", xyz[0], xyz[1], xyz[2]); return -1; } // We now have our first three joint angles // compute the hand hangles if (!ik_wrist(theta, xyz, a, s, n, 1)) { printf("unreachable wrist\n"); return 0; } for(int i = 0 ; i < 6 ; i++) printf("%f\n", theta[i] * 180 / M_PI); return 0; } #endif