# 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 124``` ```/** \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, 225 }, // to the center of the wrist joint { -90, 0, 0 }, // no translation { 90, 0, 60 }, // from the center of the wrist to the tool platform }; 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 * xyz, // position in final joint space const double * theta, // axes angles const int num_axes ) { double xyz4[4] = { xyz[0], xyz[1], xyz[2], 1 }; for (int i = num_axes-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, xyz, theta, 6); printf("%f %f %f %f\n", pos[0], pos[1], pos[2], pos[3]); return 0; } ```