puma / ik.h

 ``` 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``` ```#pragma once /** Combine the DH parameters for FK/IK and limits/scaling for drive * * 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. * * 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 { // DH parameters double d; double a; double alpha; double angle; // offset from "0" in the DH map // Mechanical details about the joint double coupled; // amount this axis is cross coupled to the previous double scale; // rads to counter ticks int min; int max; } joint_cfg_t; extern int ik_first( const joint_cfg_t * const joints, double * const theta, // output commands const double * const xyz, // desired position (in mm) const int right, // right arm = +1, left arm = -1 const int above // elbow above arm = +1, below arm = -1 ); extern int ik_wrist( const joint_cfg_t * const joints, double * const theta, // input(0-2)/output(3-5) 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 ); ```