home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl027 < prev    next >
Text File  |  1987-03-02  |  8KB  |  404 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : jacob.c
  7.  *      Remarks : Jacobian related functions.
  8.  *                The idea is not to compute twice the same thing
  9.  *                regardless of the order the functions are called.
  10.  *      Usage   : part of the library
  11.  */
  12.  
  13. /*LINTLIBRARY*/
  14.  
  15. #include "../h/rccl.h"
  16. #include "../h/which.h"
  17. #include "../h/kine.h"
  18. #include "../h/manip.h"
  19. #include "../h/umac.h"
  20.  
  21.  
  22. #ifdef PUMA
  23.  
  24. static int lasth = 0;                   /* last time h calculated       */
  25. static int lastjac = 0;                 /* last time jacob coeff .....  */
  26. static int lastu5 = 0;                  /* last time U5 ....            */
  27.  
  28.  
  29. /*
  30.  *                T  A4
  31.  * compute : T = J     F,
  32.  *                        assumes sin cos available;
  33.  */
  34.  
  35.  
  36. static jaco4T(j, f) /*##*/
  37. register JNTS_PTR j;
  38. register FORCE_PTR f;
  39. {
  40.     if (rtime != lasth) {
  41.         lasth = rtime;
  42.         GETH;
  43.     }
  44.     if (rtime != lastjac) {
  45.         lastjac = rtime;
  46.         UPDJ;
  47.     }
  48.     j->th1 = sncs_d.d1x * f->f.x +
  49.          sncs_d.d1y * f->f.y +
  50.          sncs_d.d1z * f->f.z +
  51.          sncs_d.r1x * f->m.x -
  52.          sncs_d.c23 * f->m.y +
  53.          sncs_d.r1z * f->m.z;
  54.  
  55.     j->th2 = sncs_d.d2x * f->f.x +
  56.          sncs_d.d2z * f->f.z +
  57.          sncs_d.d2y * f->f.y;
  58.  
  59.     j->th3 = sncs_d.d3x * f->f.x +
  60.          sncs_d.d3y * f->f.y +
  61.          sncs_d.d3z * f->f.z +
  62.          sncs_d.s4  * f->m.x +
  63.          sncs_d.c4  * f->m.z;
  64.  
  65.     j->th2 += j->th3;
  66.  
  67.     j->th4 =           -f->m.y;
  68.  
  69.     j->th5 =            f->m.z;
  70.  
  71.     j->th6 = sncs_d.s5  * f->m.x -
  72.          sncs_d.c5  * f->m.y;
  73. }
  74.  
  75.  
  76. /*
  77.  *          A4
  78.  * compute D   = J Q,
  79.  *                        assumes sin cos avialable ;
  80.  */
  81.  
  82.  
  83. static jaco4D(d, q) /*##*/
  84. register DIFF_PTR d;
  85. register JNTS_PTR q;
  86. {
  87.     real q23 = q->th2 + q->th3;
  88.  
  89.     if (rtime != lasth) {
  90.         lasth = rtime;
  91.         GETH;
  92.     }
  93.     if (rtime != lastjac) {
  94.         lastjac = rtime;
  95.         UPDJ;
  96.     }
  97.     d->t.x = sncs_d.d1x * q->th1 +
  98.          sncs_d.d2x * q->th2 +
  99.          sncs_d.d3x * q23;
  100.  
  101.     d->t.y = sncs_d.d1y * q->th1 +
  102.          sncs_d.d2y * q->th2 +
  103.          sncs_d.d3y * q23;
  104.  
  105.     d->t.z = sncs_d.d1z * q->th1 +
  106.          sncs_d.d2z * q->th2 +
  107.          sncs_d.d3z * q23;
  108.  
  109.     d->r.x = sncs_d.r1x * q->th1 +
  110.          sncs_d.s4  * q23    +
  111.          sncs_d.s5  * q->th6;
  112.  
  113.     d->r.y= -sncs_d.c23 * q->th1 -
  114.                 q->th4 -
  115.          sncs_d.c5  * q->th6;
  116.  
  117.     d->r.z = sncs_d.r1z * q->th1 +
  118.          sncs_d.c4  * q23    +
  119.                 q->th5;
  120. }
  121.  
  122.  
  123. /*
  124.  *                T T6
  125.  * compute : T = J    F,
  126.  *                        assumes sin cos jacob coeff available;
  127.  */
  128.  
  129. jacobT_n(j, f) /*::*/
  130. JNTS_PTR j;
  131. FORCE_PTR f;
  132. {
  133.     FORCE fl4;
  134.  
  135.     if (lastu5 != rtime) {
  136.         GETU5;
  137.         lastu5 = rtime;
  138.     }
  139.     fl4.f.x =
  140.     sncs_d.u5.n.x * f->f.x + sncs_d.u5.o.x * f->f.y + sncs_d.u5.a.x * f->f.z;
  141.     fl4.f.y =
  142.     sncs_d.u5.n.y * f->f.x + sncs_d.u5.o.y * f->f.y + sncs_d.u5.a.y * f->f.z;
  143.     fl4.f.z =
  144.     sncs_d.u5.n.z * f->f.x + sncs_d.u5.o.z * f->f.y;
  145.     fl4.m.x =
  146.     sncs_d.u5.n.x * f->m.x + sncs_d.u5.o.x * f->m.y + sncs_d.u5.a.x * f->m.z;
  147.     fl4.m.y =
  148.     sncs_d.u5.n.y * f->m.x + sncs_d.u5.o.y * f->m.y + sncs_d.u5.a.y * f->m.z;
  149.     fl4.m.z =
  150.     sncs_d.u5.n.z * f->m.x + sncs_d.u5.o.z * f->m.y;
  151.  
  152.     jaco4T(j, &fl4);
  153. }
  154.  
  155.  
  156. /*
  157.  *          T6
  158.  * compute D   = J Q,
  159.  *                        assumes sin cos jacob coeff available;
  160.  */
  161.  
  162. jacobD_n(d, q) /*::*/
  163. register DIFF_PTR d;
  164. register JNTS_PTR q;
  165. {
  166.     DIFF dl4;
  167.  
  168.     jaco4D(&dl4, q);
  169.     if (lastu5 != rtime) {
  170.         GETU5;
  171.         lastu5 = rtime;
  172.     }
  173.     d->t.x =
  174.     sncs_d.u5.n.x * dl4.t.x + sncs_d.u5.n.y * dl4.t.y + sncs_d.u5.n.z * dl4.t.z;
  175.     d->t.y =
  176.     sncs_d.u5.o.x * dl4.t.x + sncs_d.u5.o.y * dl4.t.y + sncs_d.u5.o.z * dl4.t.z;
  177.     d->t.z =
  178.     sncs_d.u5.a.x * dl4.t.x + sncs_d.u5.a.y * dl4.t.y;
  179.     d->r.x =
  180.     sncs_d.u5.n.x * dl4.r.x + sncs_d.u5.n.y * dl4.r.y + sncs_d.u5.n.z * dl4.r.z;
  181.     d->r.y =
  182.     sncs_d.u5.o.x * dl4.r.x + sncs_d.u5.o.y * dl4.r.y + sncs_d.u5.o.z * dl4.r.z;
  183.     d->r.z =
  184.     sncs_d.u5.a.x * dl4.r.x + sncs_d.u5.a.y * dl4.r.y;
  185. }
  186.  
  187.  
  188.  
  189. /*
  190.  * Compute an approximation of differential joint changes given
  191.  * Cartesian changes, ignores shoulder and elbow offsets.
  192.  *                              assumes sin cos availables
  193.  */
  194.  
  195. static jacobI4(q, d) /*::*/
  196. register JNTS_PTR q;
  197. register DIFF_PTR d;
  198. {
  199.     register int code = 0;
  200.     real q23;
  201.  
  202.     if (rtime != lasth) {
  203.         lasth = rtime;
  204.         GETH;
  205.     }
  206.  
  207.     /* theta2 */
  208.  
  209.     if (FABS(sncs_d.c3) < SMALL) {
  210.         code |= ELBOW_DEG;
  211.     }
  212.     else {
  213.         q->th2 = d->t.y / (armk_c.a2 * sncs_d.c3);
  214.     }
  215.  
  216.     /* theta1 */
  217.  
  218.     if (FABS(sncs_d.h) < SMALL) {
  219.         code |= ALIGN_DEG;
  220.     }
  221.     else {
  222.         q->th1 = (sncs_d.s4 * d->t.x + sncs_d.c4 * d->t.z);
  223.     }
  224.  
  225.     /* theta3 */
  226.  
  227.     if (!(code & ELBOW_DEG)) {
  228.         q23 = (sncs_d.c4 * d->t.x - sncs_d.s4 * d->t.z -
  229.                armk_c.a2 * sncs_d.s3 * q->th2) / armk_c.d4;
  230.         q->th3 = q23 - q->th2;
  231.     }
  232.  
  233.     /* theta6 */
  234.  
  235.     if (FABS(sncs_d.s5) < SMALL) {
  236.         code |= WRIST_DEG;
  237.     }
  238.     else {
  239.         if (!((code & ELBOW_DEG) || (code & ALIGN_DEG))) {
  240.             q->th6 = (d->r.x + sncs_d.s23 * sncs_d.c4 * q->th1 -
  241.                   sncs_d.s23 * sncs_d.c4 * q23) / sncs_d.s5;
  242.         }
  243.     }
  244.  
  245.     /* theta4 */
  246.  
  247.     if (!code) {
  248.         q->th4 = -(d->r.y + sncs_d.c23 * q->th1 + sncs_d.c5 * q->th6);
  249.  
  250.         /* theta5 */
  251.  
  252.         q->th5 = d->r.z - sncs_d.s23 * sncs_d.s4 * q->th1 - sncs_d.c4 * q23;
  253.     }
  254.     return(code);
  255. }
  256.  
  257.  
  258. jacobI_n(q, d) /*::*/
  259. register JNTS_PTR q;
  260. register DIFF_PTR d;
  261. {
  262.     DIFF dl4;
  263.  
  264.     if (lastu5 != rtime) {
  265.         GETU5;
  266.         lastu5 = rtime;
  267.     }
  268.  
  269.     dl4.t.x =
  270.     sncs_d.u5.n.x * d->t.x + sncs_d.u5.o.x * d->t.y + sncs_d.u5.a.x * d->t.z;
  271.     dl4.t.y =
  272.     sncs_d.u5.n.y * d->t.x + sncs_d.u5.o.y * d->t.y + sncs_d.u5.a.y * d->t.z;
  273.     dl4.t.z =
  274.     sncs_d.u5.n.z * d->t.x + sncs_d.u5.o.z * d->t.y;
  275.     dl4.r.x =
  276.     sncs_d.u5.n.x * d->r.x + sncs_d.u5.o.x * d->r.y + sncs_d.u5.a.x * d->r.z;
  277.     dl4.r.y =
  278.     sncs_d.u5.n.y * d->r.x + sncs_d.u5.o.y * d->r.y + sncs_d.u5.a.y * d->r.z;
  279.     dl4.r.z =
  280.     sncs_d.u5.n.z * d->r.x + sncs_d.u5.o.z * d->r.y;
  281.  
  282.     return(jacobI4(q, &dl4));
  283. }
  284.  
  285.  
  286.  
  287.  
  288. /*
  289.  * computes gravity loadings
  290.  */
  291.  
  292. gravload_n(l, c2, c23, s23, c4, s4, c5, s5) /*::*/
  293. register JNTS_PTR l;
  294. real c2, c23, s23, c4, s4, c5, s5;
  295. {
  296.     l->th1 = 0.;
  297.     l->th3 = armk_c.cp32 * s23 + armk_c.cp31 * c23;
  298.     l->th2 = l->th3 + armk_c.cp21 * c2;
  299.     l->th4 = -(armk_c.cp50 * s23 * s4 * s5);
  300.     l->th5 = armk_c.cp50 * (s23 * c4 * c5 + c23 * s5);
  301.     l->th6 = 0.;
  302. }
  303. #endif
  304.  
  305.  
  306. #ifdef STAN
  307.  
  308. static int lastjac = 0;
  309.  
  310. jacobT_n(j, f) /*::*/
  311. register JNTS_PTR j;
  312. register FORCE_PTR f;
  313. {
  314.     if (rtime != lastjac) {
  315.         lastjac = rtime;
  316.         UPDJ;
  317.     }
  318.  
  319.     j->th1 = sncs_d.d1x * f->f.x +
  320.          sncs_d.d1y * f->f.y +
  321.          sncs_d.d1z * f->f.z +
  322.          sncs_d.r1x * f->m.x +
  323.          sncs_d.r1y * f->m.y +
  324.          sncs_d.r1z * f->m.z;
  325.  
  326.     j->th2 = sncs_d.d2x * f->f.x +
  327.          sncs_d.d2y * f->f.y +
  328.          sncs_d.d2z * f->f.z +
  329.          sncs_d.r2x * f->m.x +
  330.          sncs_d.r2y * f->m.y +
  331.          sncs_d.r2z * f->m.z;
  332.  
  333.     j->th3 = sncs_d.d3x * f->f.x +
  334.          sncs_d.d3y * f->f.y +
  335.          sncs_d.d3z * f->f.z;
  336.  
  337.     j->th4 = sncs_d.r4x * f->m.x +
  338.          sncs_d.r4y * f->m.y +
  339.          sncs_d.c5  * f->m.z;
  340.  
  341.     j->th5 = sncs_d.s6  * f->m.x +
  342.          sncs_d.c6  * f->m.y;
  343.  
  344.     j->th6 =            f->m.z;
  345. }
  346.  
  347.  
  348. /*
  349.  * compute jacobian
  350.  */
  351.  
  352. jacobD_n(d, q) /*::*/
  353. register DIFF_PTR d;
  354. register JNTS_PTR q;
  355. {
  356.     if (rtime != lastjac) {
  357.         lastjac = rtime;
  358.         UPDJ;
  359.     }
  360.     d->t.x = sncs_d.d1x * q->th1 +
  361.          sncs_d.d2x * q->th2 +
  362.          sncs_d.d3x * q->th3;
  363.  
  364.     d->t.y = sncs_d.d1y * q->th1 +
  365.          sncs_d.d2y * q->th2 +
  366.          sncs_d.d3y * q->th3;
  367.  
  368.     d->t.z = sncs_d.d1z * q->th1 +
  369.          sncs_d.d2z * q->th2 +
  370.          sncs_d.d3z * q->th3;
  371.  
  372.     d->r.x = sncs_d.r1x * q->th1 +
  373.          sncs_d.r2x * q->th2 +
  374.          sncs_d.r4x * q->th4 +
  375.          sncs_d.s6  * q->th5;
  376.  
  377.     d->r.y = sncs_d.r1y * q->th1 +
  378.          sncs_d.r2y * q->th2 +
  379.          sncs_d.r4y * q->th4 +
  380.          sncs_d.c6  * q->th5;
  381.  
  382.     d->r.z = sncs_d.r1z * q->th1 +
  383.          sncs_d.r2z * q->th2 +
  384.          sncs_d.c5  * q->th4 +
  385.                 q->th6;
  386. }
  387.  
  388.  
  389. jacobI_n(q, d) /*::*/
  390. JNTS_PTR q;
  391. DIFF_PTR d;
  392. {
  393.     /* not implemented */
  394. }
  395.  
  396.  
  397. gravload_n(l, s2, c2, d3, c4, s4, c5, s5) /*::*/
  398. register JNTS_PTR l;
  399. real s2, c2, d3, c4, s4, c5, s5;
  400. {
  401.     /* not implemented */
  402. }
  403. #endif
  404.