home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL035 < prev    next >
Encoding:
Text File  |  1987-03-02  |  10.4 KB  |  514 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : solut.c
  7.  *      Remarks : kinematic functions (solve handpos).
  8.  *      Usage   : part of the library
  9.  */
  10.  
  11. /*
  12.  * jns_to_tr is handpos, it also computes the configuration (in a string )
  13.  * solveconf just compute the configuration
  14.  * tr_to_jns is solve, it has  static conf flags and get the changes
  15.  */
  16.  
  17.  
  18. #include "../h/which.h"
  19. #include "../h/rccl.h"
  20. #include "../h/manip.h"
  21. #include "../h/kine.h"
  22. #include "../h/umac.h"
  23.  
  24.  
  25. /* performs mod 2 pi */
  26.  
  27. static double range(a) /*##*/
  28. real a;
  29. {
  30.     if (a > 15. || a < -15.)
  31.         return(10.);
  32.  
  33.     while(a >= pit2_m)
  34.         a -= pit2_m;
  35.     while(a < 0.)
  36.         a += pit2_m;
  37.     if (a > pit2_m)
  38.         return(0.);
  39.     return(a);
  40. }
  41.  
  42.  
  43. #ifdef PUMA
  44.  
  45. jns_to_tr_n(t, j, u) /*::*/
  46. register TRSF_PTR t;
  47. register JNTS_PTR j;
  48. bool u;
  49. {
  50.     real c1, s1, c2, s2, c23, s23, c4, s4, c5, s5, c6, s6,
  51.     k1, k2, k3, k4, k5, k6, k7, x;
  52.  
  53.     SINCOS(s1, c1, j->th1 + jmin_c.th1);
  54.     SINCOS(s2, c2, j->th2 + jmin_c.th2);
  55.     SINCOS(s23, c23, j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3);
  56.     SINCOS(s4, c4, j->th4 + jmin_c.th4);
  57.     SINCOS(s5, c5, j->th5 + jmin_c.th5);
  58.     SINCOS(s6, c6, j->th6 + jmin_c.th6);
  59.  
  60.     k1  = c23 * s5;
  61.     k2  = c4 * c5 * s6 + s4 * c6;
  62.     k3  = - c23 * k2 + s23 * s5 * s6;
  63.     k4  = - s4 * c5 * s6 + c4 * c6;
  64.     k5  = s4 * s5;
  65.     k6 = c23 * c4 * s5 + s23 * c5;
  66.     k7 = armk_c.d4 * s23 + armk_c.a3 * c23 + armk_c.a2 * c2;
  67.  
  68.     t->o.x = c1 * k3 - s1 * k4;
  69.     t->o.y = s1 * k3 + c1 * k4;
  70.     t->o.z = s23 * k2 + k1 * s6;
  71.  
  72.     t->a.x = c1 * k6 - s1 * k5;
  73.     t->a.y = s1 * k6 + c1 * k5;
  74.     t->a.z = - s23 * c4 * s5 + c23 * c5;
  75.  
  76.     t->p.x = c1 * k7 - s1 * armk_c.d3;
  77.     t->p.y = s1 * k7 + c1 * armk_c.d3;
  78.     t->p.z = armk_c.d4 * c23 - armk_c.a3 * s23 - armk_c.a2 * s2;
  79.  
  80.     /* n = o X a */
  81.  
  82.     t->n.x = t->o.y * t->a.z - t->o.z * t->a.y;
  83.     t->n.y = t->o.z * t->a.x - t->o.x * t->a.z;
  84.     t->n.z = t->o.x * t->a.y - t->o.y * t->a.x;
  85.  
  86.     x = range(atan2(t->p.y, t->p.x) - j->th1 - jmin_c.th1);
  87.     (j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
  88.  
  89.     x = range(armk_c.aa3d4 - j->th3 - jmin_c.th3);
  90.     (j->conf)[1] = (x < pib2_m || x > pi_m + pib2_m) ? 'u' : 'd';
  91.  
  92.     (j->conf)[2] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
  93.     (j->conf)[3] = '\0';
  94.  
  95.     if (u) {
  96.         sncs_d.s1 = s1;
  97.         sncs_d.c1 = c1;
  98.         sncs_d.s2 = s2;
  99.         sncs_d.c2 = c2;
  100.         sncs_d.s23 = s23;
  101.         sncs_d.c23 = c23;
  102.         SINCOS(sncs_d.s3, sncs_d.c3, j->th3 + jmin_c.th3);
  103.         sncs_d.s4 = s4;
  104.         sncs_d.c4 = c4;
  105.         sncs_d.s5 = s5;
  106.         sncs_d.c5 = c5;
  107.         sncs_d.s6 = s6;
  108.         sncs_d.c6 = c6;
  109.     }
  110. }
  111.  
  112.  
  113.  
  114. tr_to_jns_n(j, t, u) /*::*/
  115. register JNTS_PTR j;
  116. register TRSF_PTR t;
  117. {
  118.     int code = 0;
  119.     real t1d, f11p, d, t3d, f13a, f11a, f31a, f13o, f11o, w1, w2, wd,
  120.     s1, c1, s23, c23, s3, c3, s4, c4, s5, c5, s6, c6,
  121.     px, py, pz, ax, ay, az, ox, oy, oz,
  122.     t1, t2, t3, t4, t5, t6;
  123.     static real t4o;
  124.     static lefty = NO, up = YES, flip = NO;
  125.     char *c = j->conf;
  126.  
  127.     while (*c) {
  128.         switch (*c++) {
  129.         case 'l' :
  130.             lefty = YES;
  131.             break;
  132.         case 'r' :
  133.             lefty = NO;
  134.             break;
  135.  
  136.         case 'u' :
  137.             up = YES;
  138.             break;
  139.  
  140.         case 'd' :
  141.             up = NO;
  142.             break;
  143.  
  144.         case 'f' :
  145.             flip  = YES;
  146.             break;
  147.  
  148.         case 'n' :
  149.             flip  = NO;
  150.             break;
  151.  
  152.         default  :
  153.             break;
  154.         }
  155.     }
  156.     px = t->p.x;
  157.     py = t->p.y;
  158.     pz = t->p.z;
  159.     ax = t->a.x;
  160.     ay = t->a.y;
  161.     az = t->a.z;
  162.     ox = t->o.x;
  163.     oy = t->o.y;
  164.     oz = t->o.z;
  165.  
  166.     /* theta1 */
  167.  
  168.     t1d = sqrt(px * px + py * py - armk_c.d32);
  169.     t1 = atan2(py, px) - atan2(armk_c.d3, (lefty) ? t1d : - t1d);
  170.  
  171.     if ((j->th1 = range (t1 - jmin_c.th1)) > jrng_c.th1)
  172.         code |= 01;
  173.  
  174.     SINCOS(s1, c1, t1);
  175.  
  176.     /* theta3 */
  177.  
  178.     f11p = c1 * px + s1 * py;
  179.     d = f11p * f11p + pz * pz - armk_c.e432;
  180.     t3d = sqrt(armk_c.e4aa4ad - d * d);
  181.     t3 = armk_c.aa3d4 - atan2(d, (up) ? t3d : - t3d);
  182.  
  183.     if ((j->th3 = range (t3 - jmin_c.th3)) > jrng_c.th3)
  184.         code |= 04;
  185.  
  186.     SINCOS(s3, c3, t3);
  187.  
  188.     /* theta2 */
  189.  
  190.     w1 = armk_c.a2 * c3 + armk_c.a3;
  191.     w2 = armk_c.d4 + armk_c.a2 * s3;
  192.     wd = f11p * f11p + pz * pz;
  193.     if (FABS(wd) < SMALL) {
  194.         return(01000);
  195.     }
  196.     s23 = (w2 * f11p - w1 * pz) / wd;
  197.     c23 = (w1 * f11p + w2 * pz) / wd;
  198.     t2 = atan2(s23, c23) - t3;
  199.  
  200.     if ((j->th2 = range (t2 - jmin_c.th2)) > jrng_c.th2)
  201.         code |= 02;
  202.  
  203.     /* theta4 */
  204.  
  205.     f13a = - s1 * ax + c1 * ay;
  206.     f11a =   c1 * ax + s1 * ay;
  207.     f31a = c23 * f11a - s23 * az;
  208.     t4 = (flip) ? atan2(- f13a, - f31a) : atan2(  f13a,  f31a);
  209.  
  210.     if ((j->th4 = range (t4 - jmin_c.th4)) > jrng_c.th4)
  211.         code |= 010;
  212.  
  213.     SINCOS(s4, c4, t4);
  214.  
  215.     /* theta5 */
  216.  
  217.     s5 = c4 * f31a + s4 * f13a ;
  218.     c5 = s23 * f11a + c23 * az;
  219.  
  220.     t5 = atan2(s5, c5);
  221.  
  222.     if (FABS(t5) < SMALL) {
  223.         j->th4 = range(t4o - jmin_c.th4);
  224.         SINCOS(s4, c4, t4o);
  225.         s5 = 0.;
  226.         c5 = 1.;
  227.         code |= 0400;
  228.     }
  229.     else {
  230.         t4o = t4;
  231.     }
  232.  
  233.     if ((j->th5 = range (t5 - jmin_c.th5)) > jrng_c.th5)
  234.         code |= 020;
  235.  
  236.     /* theta6 */
  237.  
  238.     f13o = - s1 * ox + c1 * oy;
  239.     f11o =   c1 * ox + s1 * oy;
  240.     s6 = - c5 * (c4 * (c23 * f11o - s23 * oz) + s4 * f13o)
  241.          + s5 * (s23 * f11o + c23 * oz);
  242.     c6 = - s4 * (c23 * f11o - s23 * oz) + c4 * f13o;
  243.  
  244.     t6 = atan2(s6, c6);
  245.  
  246.     if ((j->th6 = range (t6 - jmin_c.th6)) > jrng_c.th6)
  247.         code |= 040;
  248.  
  249.     if (u) {
  250.         sncs_d.s1 = s1;
  251.         sncs_d.c1 = c1;
  252.         SINCOS(sncs_d.s2, sncs_d.c2, t2);
  253.         sncs_d.s23 = s23;
  254.         sncs_d.c23 = c23;
  255.         sncs_d.s3 = s3;
  256.         sncs_d.c3 = c3;
  257.         sncs_d.s4 = s4;
  258.         sncs_d.c4 = c4;
  259.         sncs_d.s5 = s5;
  260.         sncs_d.c5 = c5;
  261.         sncs_d.s6 = s6;
  262.         sncs_d.c6 = c6;
  263.     }
  264.     return(code);
  265. }
  266.  
  267.  
  268.  
  269. solveconf_n(j) /*::*/
  270. register JNTS_PTR j;
  271. {
  272.     real s1, c1, s23, c23, c2, px, py, k, x;
  273.  
  274.     SINCOS(s1, c1, j->th1 + jmin_c.th1);
  275.     SINCOS(s23, c23, j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3);
  276.     c2 = cos(j->th2 + jmin_c.th2);
  277.  
  278.     k = armk_c.d4 * s23 + armk_c.a3 * c23 + armk_c.a2 * c2;
  279.     px = c1 * k - s1 * armk_c.d3;
  280.     py = s1 * k + c1 * armk_c.d3;
  281.  
  282.     x = range(atan2(py, px) - j->th1 - jmin_c.th1);
  283.     (j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
  284.  
  285.     x = range(armk_c.aa3d4 - j->th3 - jmin_c.th3);
  286.     (j->conf)[1] = (x < pib2_m || x > pi_m + pib2_m) ? 'u' : 'd';
  287.  
  288.     (j->conf)[2] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
  289.     (j->conf)[3] = '\0';
  290. }
  291. #endif
  292.  
  293.  
  294. #ifdef STAN
  295.  
  296. jns_to_tr_n(t, j, u) /*::*/
  297. register TRSF_PTR t;
  298. register JNTS_PTR j;
  299. bool u;
  300. {
  301.     real c1, s1, c2, s2, d3, c4, s4, c5, s5, c6, s6,
  302.     k1, k2, k3, k4, k5, k6, k7, x;
  303.  
  304.     SINCOS(s1, c1, j->th1 + jmin_c.th1);
  305.     SINCOS(s2, c2, j->th2 + jmin_c.th2);
  306.     SINCOS(s4, c4, j->th4 + jmin_c.th4);
  307.     SINCOS(s5, c5, j->th5 + jmin_c.th5);
  308.     SINCOS(s6, c6, j->th6 + jmin_c.th6);
  309.  
  310.     d3 = j->th3 + jmin_c.th3;
  311.     k1 = c4 * s5;
  312.     k2 = s4 * s5;
  313.     k3 = c4 * c5 * s6 + s4 * c6;
  314.     k4 = - c2 * k3 + s2 * s5 * s6;
  315.     k5 = - s4 * c5 * s6 + c4 * c6;
  316.     k6 = c2 * k1 + s2 * c5;
  317.     k7 = s2 * d3;
  318.  
  319.     t->o.x = c1 * k4 - s1 * k5;
  320.     t->o.y = s1 * k4 + c1 * k5;
  321.     t->o.z = s2 * k3 + c2 * s5 * s6;
  322.  
  323.     t->a.x = c1 * k6 - s1 * k2;
  324.     t->a.y = s1 * k6 + c1 * k2;
  325.     t->a.z = - s2 * k1 + c2 * c5;
  326.  
  327.     t->p.x = c1 * k7 - s1 * armk_c.d2;
  328.     t->p.y = s1 * k7 + c1 * armk_c.d2;
  329.     t->p.z = c2 * d3;
  330.  
  331.     /* n = o X a */
  332.  
  333.     t->n.x = t->o.y * t->a.z - t->o.z * t->a.y;
  334.     t->n.y = t->o.z * t->a.x - t->o.x * t->a.z;
  335.     t->n.z = t->o.x * t->a.y - t->o.y * t->a.x;
  336.  
  337.     x = range(atan2(t->p.y, t->p.x) - j->th1 - jmin_c.th1);
  338.     (j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
  339.  
  340.     (j->conf)[1] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
  341.     (j->conf)[2] = '\0';
  342.  
  343.     if (u) {
  344.         sncs_d.s1 = s1;
  345.         sncs_d.c1 = c1;
  346.         sncs_d.s2 = s2;
  347.         sncs_d.c2 = c2;
  348.         sncs_d.d3 = d3;
  349.         sncs_d.s4 = s4;
  350.         sncs_d.c4 = c4;
  351.         sncs_d.s5 = s5;
  352.         sncs_d.c5 = c5;
  353.         sncs_d.s6 = s6;
  354.         sncs_d.c6 = c6;
  355.     }
  356. }
  357.  
  358.  
  359.  
  360. tr_to_jns_n(j, t, u) /*::*/
  361. register JNTS_PTR j;
  362. register TRSF_PTR t;
  363. bool u;
  364. {
  365.     int code = 0;
  366.     real s1, c1, s2, c2, s4, c4, s5, c5, s6, c6,
  367.     f11p, f13a, f11a, f21a, f11o, f21o, f22o,
  368.     px, py, pz, ax, ay, az, ox, oy, oz,
  369.     t1d, t1, t2, d3, t4, t5, t6;
  370.     static real t4o;
  371.     static int lefty = YES, flip = NO;
  372.     char *c = j->conf;
  373.  
  374.     while (*c) {
  375.         switch (*c++) {
  376.         case 'l' :
  377.             lefty = YES;
  378.             break;
  379.         case 'r' :
  380.             lefty = NO;
  381.             break;
  382.  
  383.         case 'f' :
  384.             flip  = YES;
  385.             break;
  386.  
  387.         case 'n' :
  388.             flip  = NO;
  389.             break;
  390.  
  391.         default  :
  392.             break;
  393.         }
  394.     }
  395.     px = t->p.x;
  396.     py = t->p.y;
  397.     pz = t->p.z;
  398.     ax = t->a.x;
  399.     ay = t->a.y;
  400.     az = t->a.z;
  401.     ox = t->o.x;
  402.     oy = t->o.y;
  403.     oz = t->o.z;
  404.  
  405.     /* theta1 */
  406.  
  407.     t1d = sqrt(px * px + py * py - armk_c.d22);
  408.     t1 = atan2(py, px) - atan2(armk_c.d2, (lefty) ? t1d : - t1d);
  409.  
  410.     if ((j->th1 = range (t1 - jmin_c.th1)) > jrng_c.th1)
  411.         code |= 01;
  412.  
  413.     SINCOS(s1, c1, t1);
  414.  
  415.     /* theta2 */
  416.  
  417.     t2 = atan2(f11p = c1 * px + s1 * py, pz);
  418.  
  419.     if ((j->th2 = range (t2 - jmin_c.th2)) > jrng_c.th2)
  420.         code |= 02;
  421.  
  422.     SINCOS(s2, c2, t2);
  423.  
  424.     /* dist3 */
  425.  
  426.     d3 = s2 * f11p + c2 * pz;
  427.  
  428.     if ((j->th3 = d3 - jmin_c.th3) > jrng_c.th3 || j->th3 < 0.)
  429.         code |= 04;
  430.  
  431.     /* theta4 */
  432.  
  433.     f13a = - s1 * ax + c1 * ay;
  434.     f11a =   c1 * ax + s1 * ay;
  435.     f21a = c2 * f11a - s2 * az;
  436.     t4 = (flip) ? atan2(- f13a, - f21a) : atan2(f13a,  f21a);
  437.  
  438.     if ((j->th4 = range (t4 - jmin_c.th4)) > jrng_c.th4)
  439.         code |= 010;
  440.  
  441.     SINCOS(s4, c4, t4);
  442.  
  443.     /* theta5 */
  444.  
  445.     s5 = c4 * f21a + s4 * f13a ;
  446.     c5 = s2 * f11a + c2 * az;
  447.  
  448.     t5 = atan2(s5, c5);
  449.  
  450.     if (FABS(t5) < SMALL) {
  451.         j->th4 = range(t4o - jmin_c.th4);
  452.         SINCOS(s4, c4, t4o);
  453.         s5 = 0.;
  454.         c5 = 1.;
  455.         code |= 0400;
  456.     }
  457.     else {
  458.         t4o = t4;
  459.     }
  460.     if ((j->th5 = range (t5 - jmin_c.th5)) > jrng_c.th5)
  461.         code |= 020;
  462.  
  463.     /* theta6 */
  464.  
  465.     f11o =   c1 * ox + s1 * oy;
  466.     f21o = c2 * f11o - s2 * oz;
  467.     f22o = - s1 * ox + c1 * oy;
  468.  
  469.     s6 = - c5 * (c4 * f21o + s4 * f22o)
  470.          + s5 * (s2 * f11o + c2 * oz);
  471.     c6 = - s4 * f21o + c4 * f22o;
  472.     t6 = atan2(s6, c6);
  473.  
  474.     if ((j->th6 = range (t6 - jmin_c.th6)) > jrng_c.th6)
  475.         code |= 040;
  476.  
  477.     if (u) {
  478.         sncs_d.s1 = s1;
  479.         sncs_d.c1 = c1;
  480.         sncs_d.s2 = s2;
  481.         sncs_d.c2 = c2;
  482.         sncs_d.d3 = d3;
  483.         sncs_d.s4 = s4;
  484.         sncs_d.c4 = c4;
  485.         sncs_d.s5 = s5;
  486.         sncs_d.c5 = c5;
  487.         sncs_d.s6 = s6;
  488.         sncs_d.c6 = c6;
  489.     }
  490.  
  491.     return(code);
  492. }
  493.  
  494.  
  495. solveconf_n(j) /*::*/
  496. register JNTS_PTR j;
  497. {
  498.     real s1, c1, s2, px, py, x;
  499.  
  500.     SINCOS(s1, c1, j->th1 + jmin_c.th1);
  501.     s2 = sin(j->th2 + jmin_c.th2);
  502.     x  = s2 * (j->th3 + jmin_c.th3);
  503.  
  504.     px = c1 * x - s1 * armk_c.d2;
  505.     py = s1 * x + c1 * armk_c.d2;
  506.  
  507.     x = range(atan2(py, px) - j->th1 - jmin_c.th1);
  508.     (j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';
  509.  
  510.     (j->conf)[1] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
  511.     (j->conf)[2] = '\0';
  512. }
  513. #endif
  514.