# puma / fk.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 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123``` ```/** \file * Forward Kinesmatics for the PUMA arms. */ #include #include #include #include #include "matrix.h" /** * Rotations are right-hand rule! (Counter clockwise) * * 1. Translate by d along the new z axis. * 2. Rotate by the new theta about the new z axis. * 3. Translate by a along the old x axis. * 4. Rotate by a about the old x axis. * * This does not handle cross coupling! * * Note that for the PUMA the straight-up home position is * [0, -90, 90, 0, 0, 0], not all zeros. This is since the X * axis is aligned with the horizontal plane. */ typedef struct { double alpha; // rotation around old x axis double a; // translation along old x axis double d; // translation along new z axis } dh_param_t; static const dh_param_t dh_params[] = { { 0, 0, 0 }, // origin is at center of shoulder { -90, 0, 70 }, // { 0, 205, 60 }, { 90, 0, 205 }, { -90, 0, 00 }, { 90, 0, 0 }, }; static m44_t fk_dh_build( const dh_param_t * dh, const double theta ) { const double ct = cos(theta); const double st = sin(theta); const double ca = cos(dh->alpha * M_PI/180); const double sa = sin(dh->alpha * M_PI/180); return (m44_t) { .v = { { ct, -st, 0, dh->a }, { st*ca, ct*ca, -sa, -sa*dh->d }, { st*sa, ct*sa, ca, ca*dh->d }, { 0, 0, 0, 1 }, }}; } static void m44_print( const m44_t * const m ) { int i, j; for (i=0 ; i < 4 ; i++) { for (j=0 ; j < 4 ; j++) printf(" %.3f", m->v[i][j]); printf("\n"); } } int fk( double * pos_out, const double * theta, // six axes angles const double * xyz // position in final joint space ) { double xyz4[4] = { xyz[0], xyz[1], xyz[2], 1 }; for (int i = 6-1 ; i >= 0 ; i--) { m44_t t = fk_dh_build(&dh_params[i], theta[i]); //printf("%d: %f\n", i, theta[i]); //m44_print(&t); //printf("\n"); m44_mult4v(pos_out, &t, xyz4); xyz4[0] = pos_out[0]; xyz4[1] = pos_out[1]; xyz4[2] = pos_out[2]; xyz4[3] = pos_out[3]; } return 0; } int main(int argc, char **argv) { double theta[6]; if (argc != 7) { fprintf(stderr, "need 6 axis angles\n"); return -1; } for (int i = 1 ; i < 7 ; i++) theta[i-1] = atof(argv[i]) * M_PI / 180; double xyz[3] = { 0, 0, 0 }; double pos[4]; fk(pos, theta, xyz); printf("%f %f %f %f\n", pos[0], pos[1], pos[2], pos[3]); return 0; } ```
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.