home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 471 / rccl161 < prev    next >
Text File  |  1987-03-02  |  4KB  |  134 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : h
  6.  *      File    : kine.h
  7.  *      Remarks : Very dependent on arm type
  8.  *      Usage   : Included in concerned files, made available to the user.
  9.  */
  10.  
  11. #ifdef PUMA
  12.  
  13. #define ELBOW_DEG       01              /* elbow degeneracy             */
  14. #define ALIGN_DEG       02              /* T6 in X Z Jt 1 plan          */
  15. #define WRIST_DEG       03              /* wrist degeneracy             */
  16.  
  17. typedef struct kindyn {
  18.            real a2, a3, d3, d4, d32, e432, aa3d4, e4aa4ad;
  19.            real cp21, cp31, cp32, cp50;
  20. } KINDYN, *KINDYN_PTR;
  21.  
  22. typedef struct sincos {
  23.     real c1, s1, c2, s2, c23, s23, c3, s3, c4, s4, c5, s5, c6, s6;
  24.     real d1x, d1y, d1z, r1x, r1z, d2x, d2y, d2z, d3x, d3y, d3z;
  25.     real h;
  26.     TRSF u5;
  27. } SNCS, *SNCS_PTR;
  28.  
  29. /*
  30.  * Macro updates coef of Jacob from the sin cos
  31.  */
  32.  
  33. #define GETH\
  34. {\
  35.     sncs_d.h = sncs_d.c2 * armk_c.a2 +\
  36.            sncs_d.s23 * armk_c.d4 +\
  37.            sncs_d.c23 * armk_c.a3;\
  38. }
  39. #define UPDJ\
  40. {\
  41.     sncs_d.d1x = sncs_d.h * sncs_d.s4 -\
  42.              armk_c.d3 * sncs_d.c23 * sncs_d.c4;\
  43.     sncs_d.d1y = sncs_d.s23 * armk_c.d3;\
  44.     sncs_d.d1z = sncs_d.h * sncs_d.c4 + armk_c.d3 * sncs_d.c23 * sncs_d.s4;\
  45.     sncs_d.r1x = -sncs_d.s23 * sncs_d.c4;\
  46.     sncs_d.r1z = sncs_d.s23 * sncs_d.s4;\
  47.     sncs_d.d2x = armk_c.a2 * sncs_d.s3 * sncs_d.c4;\
  48.     sncs_d.d2y = armk_c.a2 * sncs_d.c3;\
  49.     sncs_d.d2z = -armk_c.a2 * sncs_d.s3 * sncs_d.s4;\
  50.     sncs_d.d3x = sncs_d.c4 * armk_c.d4;\
  51.     sncs_d.d3y = armk_c.a3;\
  52.     sncs_d.d3z = -sncs_d.s4 * armk_c.d4;\
  53. }
  54.  
  55. #define GETU5\
  56. {\
  57.     sncs_d.u5.n.x = sncs_d.c5 * sncs_d.c6;\
  58.     sncs_d.u5.n.y = sncs_d.s5 * sncs_d.c6;\
  59.     sncs_d.u5.n.z = sncs_d.s6;\
  60.     sncs_d.u5.o.x= -sncs_d.c5 * sncs_d.s6;\
  61.     sncs_d.u5.o.y= -sncs_d.s5 * sncs_d.s6;\
  62.     sncs_d.u5.o.z = sncs_d.c6;\
  63.     sncs_d.u5.a.x = sncs_d.s5;\
  64.     sncs_d.u5.a.y= -sncs_d.c5;\
  65.     sncs_d.u5.a.z = 0.;\
  66. }
  67. #endif
  68.  
  69. #ifdef STAN
  70. typedef struct kindyn {
  71.     real d2, d22;
  72. } KINDYN, *KINDYN_PTR;
  73.  
  74. typedef struct sincos {
  75.     real c1, s1, c2, s2, d3, c4, s4, c5, s5, c6, s6;
  76.     real d1x, d1y, d1z, r1x, r1y, r1z, d2x, d2y, d2z, r2x, r2y, r2z,
  77.         d3x, d3y, d3z, r4x, r4y;
  78. } SNCS, *SNCS_PTR;
  79.  
  80. #define UPDJ\
  81. {\
  82.     real\
  83.     k1 = sncs_d.c4 * sncs_d.c5,\
  84.     k2 = sncs_d.s4 * sncs_d.c5,\
  85.     k3 = sncs_d.c4 * sncs_d.s5,\
  86.     k4 = sncs_d.s2 * sncs_d.d3,\
  87.     k5 = k1      * sncs_d.c6,\
  88.     k6 = sncs_d.s4 * sncs_d.c6,\
  89.     k7 = k5 - sncs_d.s4 * sncs_d.s6,\
  90.     k8 = k2 * sncs_d.c6 + sncs_d.c4 * sncs_d.s6,\
  91.     k9 = k1 * sncs_d.s6 + k6,\
  92.     k10= - k2 * sncs_d.s6 + sncs_d.c4 * sncs_d.c6,\
  93.     k11= k5 + k6,\
  94.     k12= sncs_d.s4 * sncs_d.s5,\
  95.     k13= sncs_d.s5 * sncs_d.c6,\
  96.     k14= sncs_d.s5 * sncs_d.s6;\
  97.     sncs_d.d1x = (-armk_c.d2 * (sncs_d.c2 * k7 - sncs_d.s2 * k13) +\
  98.               k4 * k8);\
  99.     sncs_d.d2x = sncs_d.d3 * k7;\
  100.     sncs_d.d3x = -k13;\
  101.     sncs_d.d1y = (-armk_c.d2 * (- sncs_d.c2 * k9 + sncs_d.s2 * k14) +\
  102.               k4 * k10);\
  103.     sncs_d.d2y = -sncs_d.d3 * k11;\
  104.     sncs_d.d3y = k14;\
  105.     sncs_d.d1z = (-armk_c.d2 * (sncs_d.c2 * k3 + sncs_d.s2 * sncs_d.c5) +\
  106.               k4 * k12);\
  107.     sncs_d.d2z = sncs_d.d3 * k3;\
  108.     sncs_d.d3z = sncs_d.c5;\
  109.     sncs_d.r1x = (-sncs_d.s2 * k7 - sncs_d.c2 * k13);\
  110.     sncs_d.r2x = k8;\
  111.     sncs_d.r4x = -k13;\
  112.     sncs_d.r1y = (sncs_d.s2 * k9 + sncs_d.c2 * k14);\
  113.     sncs_d.r2y = k10;\
  114.     sncs_d.r4y = k14;\
  115.     sncs_d.r1z = (-sncs_d.s2 * k3 + sncs_d.c2 * sncs_d.c5);\
  116.     sncs_d.r2z = k12;\
  117. }
  118. #endif
  119.  
  120.  
  121. extern  KINDYN  armk_c;                 /* arm kinematic and dynamic    */
  122.                     /* constants                    */
  123.  
  124. extern  SNCS    sncs_d;                 /* current sin cos, jacob coeff */
  125.                     /* and U5 matrix                */
  126.  
  127. extern  JNTS    jcal_c;                 /* rest position joint range    */
  128.  
  129. extern  JNTS    jmin_c;                 /* angles range offset values   */
  130.  
  131. extern  JNTS    jrng_c;                 /* maximum joint range values   */
  132.  
  133. extern  JNTS    jmxv_c;                 /* max joint velocities */
  134.