# puma / ik.c

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80``` ```/** \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 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? 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 * pz * 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); 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); 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 0; } #if 0 int main(void) { const double xyz[3] = { 250, 0, 0 }; double theta[6]; ik_first(theta, xyz, 1, 1); // right, above for(int i = 0 ; i < 3 ; i++) printf("%f\n", theta[i] * 180 / M_PI); return 0; } #endif ```
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.