home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl031 < prev    next >
Text File  |  1987-03-02  |  11KB  |  532 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : intest.c
  7.  *      Remarks : Interactive program to test the Kinematic functions.
  8.  *      Usage   : make intest
  9.  */
  10.  
  11. #include <signal.h>
  12. #include "../h/which.h"
  13. #include "../h/rccl.h"
  14. #include "../h/kine.h"
  15. #include "../h/manip.h"
  16.  
  17.  
  18. JNTS  jzero = {"          ", 0., 0., 0., 0., 0., 0.};
  19.  
  20. char *entert();
  21.  
  22. int sframp;             /* solution angles or range values      */
  23.  
  24. main()
  25. {
  26.     int exit(), c;
  27.  
  28.     signal(SIGINT, exit);
  29.  
  30.     printf("angles/range ?");
  31.     while((c = getchar()) != 'a' && c != 'r') {
  32.     }
  33.     if (c == 'a') {
  34.         sframp = YES;
  35.     }
  36.     else {
  37.         sframp = NO;
  38.     }
  39.  
  40.     solveconf_n(&jcal_c);
  41.     solveconf_n(&jrng_c);
  42.     solveconf_n(&jzero);
  43.  
  44.     printf("h / s / j / t / f / i / w / b:");
  45.     while(
  46. (c=getchar())!='h'&&c!='s'&&c!='j'&&c!='t'&&c!='f'&&c!='b'&&c!='i'&&c!='w'){
  47.     }
  48.     switch (c) {
  49.     case 'h' :
  50.         for (; ; ) {
  51.             hdp();          /* handpos      */
  52.         }
  53.     case 's' :
  54.         for (; ; ) {
  55.             sol();          /* solution     */
  56.         }
  57.     case 'j' :
  58.         for (; ; ) {
  59.             jacd();         /* jacobian     */
  60.         }
  61.     case 't' :
  62.         for (; ; ) {
  63.             jact();         /* jacobian T   */
  64.         }
  65.     case 'f' :
  66.         for (; ; ) {
  67.             jacj();         /* give j torques from Cart specs */
  68.         }
  69.     case 'i' :
  70.         for (; ; ) {
  71.             jaci();         /* jacobian -1 (approximation)  */
  72.         }
  73.     case 'w' :
  74.         for (; ; ) {
  75.             jacw();         /* compute the j works in an    */
  76.         }                       /* attempt to study a selection */
  77.     case 'b' :                      /* method                       */
  78.         for (; ; ) {
  79.             jacb();         /* test the current method      */
  80.         }
  81.     }
  82. }
  83.  
  84.  
  85. hdp()
  86. {
  87.  
  88.     TRSF tra, trb;
  89.     TRSF_PTR tma = &tra, tmb = &trb;
  90.     static JNTS jsa = {"          "}  , jsb = {"           "};
  91.     int code;
  92.  
  93.  
  94.     enterj(&jsa);
  95.     jns_to_tr_n(tma, &jsa, YES);
  96.     printsc();
  97.     printj(&jsa);
  98.     printr(tma, stdout);
  99.     putchar('\n');
  100.     strcpy(jsb.conf, jsa.conf);
  101.     printj(&jzero);
  102.     printj(&jcal_c);
  103.     printj(&jrng_c);
  104.  
  105.     code = tr_to_jns_n(&jsb, tma, YES);
  106.     if (code == 01000) {
  107.         printf("wd zero\n");
  108.         return;
  109.     }
  110.     printsc();
  111.     printf("    %c        %c        %c        %c        %c        %c\n",
  112.     (code & 01) ? '1' : '-', (code & 02) ? '2' : '-',
  113.     (code & 04) ? '3' : '-', (code & 010) ? '4' : '-',
  114.     (code & 020) ? '5' : '-', (code & 040) ? '6' : '-');
  115.     jns_to_tr_n(tmb, &jsb, NO);
  116.     printj(&jsb);
  117.     printr(tmb, stdout);
  118.     putchar('\n');
  119. }
  120.  
  121. sol()
  122. {
  123.     TRSF t6;
  124.     JNTS j6;
  125.     int code;
  126.  
  127.     j6.conf = entert(&t6);
  128.     printr(&t6, stdout);
  129.     putchar('\n');
  130.     code = tr_to_jns_n(&j6, &t6, YES);
  131.     printsc();
  132.     printj(&jzero);
  133.     printj(&jcal_c);
  134.     printj(&jrng_c);
  135.     printf("    %c        %c        %c        %c        %c        %c\n",
  136.     (code & 01) ? '1' : '-', (code & 02) ? '2' : '-',
  137.     (code & 04) ? '3' : '-', (code & 010) ? '4' : '-',
  138.     (code & 020) ? '5' : '-', (code & 040) ? '6' : '-');
  139.     printj(&j6);
  140.     jns_to_tr_n(&t6, &j6, YES);
  141.     printsc();
  142.     printr(&t6, stdout);
  143.     putchar('\n');
  144. }
  145.  
  146.  
  147.  
  148. jacd()
  149. {
  150.     static JNTS j6 = {"     "},
  151.             j6d = {"     "},
  152.             j6D = {"     "},
  153. #ifdef PUMA
  154.             jd = {"     ", .001, .001, .001, .001, .001, .001};
  155. #endif
  156. #ifdef STAN
  157.             jd = {"     ", .001, .001,  1., .001, .001, .001};
  158. #endif
  159.  
  160.  
  161.     TRSF t6, t6i, t6d, td;
  162.     DIFF d6a, d6b, d6d;
  163.  
  164.     enterj(&j6);
  165.     printj(&j6);
  166.     putchar('\n');
  167.     fojnts_n(&j6d, &jd, &j6, 1.);
  168.     diffjnts_n(&j6D, &j6d, &j6);
  169.     jns_to_tr_n(&t6, &j6, NO);
  170.     jns_to_tr_n(&t6d, &j6d, YES);
  171.     Trmult(&td, invert(&t6i, &t6), &t6d);
  172.     printd(tr_to_df(&d6a, &td), stdout);
  173.     ++rtime;
  174.     jacobD_n(&d6b, &j6D);
  175.     printd(&d6b, stdout);
  176.     diffdiff(&d6d, &d6a, &d6b);
  177.     printd(&d6d, stdout);
  178.     putchar('\n');
  179. }
  180.  
  181.  
  182. jact()
  183. {
  184.     static JNTS j6 = {"     "},
  185.             jt = {"     "};
  186.  
  187.     TRSF t6;
  188.     FORCE ft6;
  189.  
  190.     j6.conf = entert(&t6);
  191.     printr(&t6, stdout);
  192.     tr_to_jns_n(&j6, &t6, YES);
  193.     printj(&j6);
  194.     printf(
  195. "- fx fy fz mx my mz ------------------------------------------------------\n"
  196.        );
  197.     if (scanf("%f%f%f%f%f%f"
  198.      , &ft6.f.x, &ft6.f.y, &ft6.f.z, &ft6.m.x, &ft6.m.y, &ft6.m.z) == -1)
  199.         exit(0);
  200.  
  201.     ++rtime;
  202.     jacobT_n(&jt, &ft6);
  203.     printt(&jt);
  204.     putchar('\n');
  205. }
  206.  
  207.  
  208. jacj()
  209. {
  210.     static JNTS j6 = {"     "},
  211.             jt = {"     "};
  212.  
  213.     TRSF t6;
  214.     FORCE ft6;
  215.  
  216.     printj(&jcal_c);
  217.     enterj(&j6);
  218.     jns_to_tr_n(&t6, &j6, YES);
  219.     printr(&t6, stdout);
  220.     printf(
  221. "- fx fy fz mx my mz ------------------------------------------------------\n"
  222.        );
  223.     if (scanf("%f%f%f%f%f%f"
  224.      , &ft6.f.x, &ft6.f.y, &ft6.f.z, &ft6.m.x, &ft6.m.y, &ft6.m.z) == -1)
  225.         exit(0);
  226.  
  227.     ++rtime;
  228.     jacobT_n(&jt, &ft6);
  229.     printt(&jt);
  230.     putchar('\n');
  231. }
  232.  
  233.  
  234. jaci()
  235. {
  236.     static JNTS j6 = {"     "}, j6d = {"      "}, j6D = {"     "};
  237.     TRSF t6, t6d, t6D;
  238.     DIFF d6;
  239.  
  240.     printj(&jcal_c);
  241.     enterj(&j6);
  242.     jns_to_tr_n(&t6, &j6, NO);
  243.     printf("enter diff t6: x y z X Y Z\n");
  244.     scanf("%f%f%f%f%f%f",&d6.t.x, &d6.t.y, &d6.t.z, &d6.r.x, &d6.r.y, &d6.r.z);
  245.     Df_to_tr(&t6D, &d6);
  246.     Trmult(&t6d, &t6, &t6D);
  247.     strcpy(&j6d.conf, &j6.conf);
  248.     tr_to_jns_n(&j6, &t6, YES);
  249.     tr_to_jns_n(&j6d, &t6d, NO);
  250.     diffjnts_n(&j6D, &j6d, &j6);
  251.     printt(&j6D);
  252.     ++rtime;
  253.     if (jacobI_n(&j6D, &d6)) printf("degenerate\n");
  254.     printt(&j6D);
  255. }
  256.  
  257.  
  258.  
  259. jacw()
  260. {
  261.     static JNTS j6 = {"      "}, jD;
  262.     TRSF t6;
  263.     TRSF tl, tli;
  264.     DIFF d6, dtl;
  265.     JNTS jt;
  266.     FORCE f6;
  267.  
  268.     printj(&jcal_c);
  269.     enterj(&j6);
  270.     printf("enter tool\n");
  271.     entert(&tl);
  272.     jns_to_tr_n(&t6, &j6, YES);
  273.     Invert(&tli, &tl);
  274.     for (; ; ) {
  275.         printf("enter diff tl x y z X Y Z\n");
  276.         if (scanf("%f%f%f%f%f%f",
  277.     &dtl.t.x, &dtl.t.y, &dtl.t.z, &dtl.r.x, &dtl.r.y, &dtl.r.z) == -1) {
  278.             return;
  279.         }
  280.         ++rtime;
  281.         Difftr(&d6, &dtl, &tli);
  282.         if (jacobI_n(&jD, &d6)) {
  283.             printf("degenerate\n");
  284.             return;
  285.         }
  286.         printf("diff joints :\n");
  287.         printt(&jD);
  288.         Forcetr(&f6, &dtl, &tli);
  289.         jacobT_n(&jt, &f6);
  290.         printf("joint torques\n");
  291.         printt(&jt);
  292.         printf(
  293. "works \n1: %8.1e  2: %8.1e  3: %8.1e  4: %8.1e  5: %8.1e  6: %8.1e\n",
  294. jt.th1*jD.th1,jt.th2*jD.th2,jt.th3*jD.th3,jt.th4*jD.th4,jt.th5*jD.th5,jt.th6*jD.
  295.     }
  296. }
  297.  
  298.  
  299.  
  300.  
  301.  
  302. jacb()
  303. {
  304.     static JNTS j6 = {"     "};
  305.     TRSF t6, t;
  306.     VECT k;
  307.     double kx, ky, kz, theta;
  308.     int dirs = 0, choice;
  309.     int sx, sy, sz, sX, sY, sZ;
  310.  
  311.     t.p.x = t.p.y = t.p.z = 0.;
  312.     printj(&jcal_c);
  313.     enterj(&j6);
  314.     printj(&j6);
  315.     jns_to_tr_n(&t6, &j6, YES);
  316.     printr(&t6, stdout);
  317.     for (; ; ) {
  318.         dirs = 0;
  319.         printf("enter tool rot :kx ky kz theta\n");
  320.         if (scanf("%f%f%f%f", &kx, &ky, &kz, &theta) == -1) {
  321.             return;
  322.         }
  323.         k.x = kx; k.y = ky; k.z = kz;
  324.         Rot(&t, &k, theta);
  325.         printf("comply tool x y z X Y Z\n");
  326.         if (scanf("%d %d %d %d %d %d", &sx, &sy, &sz, &sX, &sY, &sZ) == -1)
  327.             exit(0);
  328.  
  329.         if (sx) dirs |= SELFX;
  330.         if (sy) dirs |= SELFY;
  331.         if (sz) dirs |= SELFZ;
  332.         if (sX) dirs |= SELMX;
  333.         if (sY) dirs |= SELMY;
  334.         if (sZ) dirs |= SELMZ;
  335.  
  336. #ifdef PUMA
  337.         GETU5;
  338.         GETH;
  339. #endif
  340.         UPDJ;
  341.         choice = select_n(dirs, &t);
  342.         printf("%o : %c %c %c %c %c %c\n", dirs,
  343.         (choice & SELJ1) ? '1' : ' ',
  344.         (choice & SELJ2) ? '2' : ' ',
  345.         (choice & SELJ3) ? '3' : ' ',
  346.         (choice & SELJ4) ? '4' : ' ',
  347.         (choice & SELJ5) ? '5' : ' ',
  348.         (choice & SELJ6) ? '6' : ' ');
  349.     }
  350. }
  351.  
  352.  
  353. enterj(j)
  354. JNTS_PTR j;
  355. {
  356.     double a[6], r[6];
  357.  
  358.     printf((sframp) ? "--solution angles---" : "--range angles-----");
  359. #ifdef PUMA
  360.     printf("---PUMA-----------------------------\n");
  361. #endif
  362. #ifdef STAN
  363.     printf("---STANFORD-ARM---------------------\n");
  364. #endif
  365.     if (scanf("%f%f%f%f%f%f"
  366.      , &a[0], &a[1], &a[2], &a[3], &a[4], &a[5]) == -1)
  367.         exit(0);
  368.  
  369.     a[0] *= dgtord_m;
  370.     a[1] *= dgtord_m;
  371. #ifdef PUMA
  372.     a[2] *= dgtord_m;
  373. #endif
  374.     a[3] *= dgtord_m;
  375.     a[4] *= dgtord_m;
  376.     a[5] *= dgtord_m;
  377.  
  378.  
  379.     if (sframp) {
  380.         angtorng(r, a);
  381.         j->th1 = r[0];
  382.         j->th2 = r[1];
  383.         j->th3 = r[2];
  384.         j->th4 = r[3];
  385.         j->th5 = r[4];
  386.         j->th6 = r[5];
  387.     }
  388.     else {
  389.         j->th1 = a[0];
  390.         j->th2 = a[1];
  391.         j->th3 = a[2];
  392.         j->th4 = a[3];
  393.         j->th5 = a[4];
  394.         j->th6 = a[5];
  395.     }
  396. }
  397.  
  398. char *entert(t)
  399. TRSF_PTR t;
  400. {
  401.  
  402.     static char conf[10];
  403.     double x, y, z, kx, ky, kz;
  404.     VECT k;
  405.     real theta;
  406.  
  407.     printf("- x  y  z --- kx ky kz - theta - conf ");
  408. #ifdef PUMA
  409.     printf("---PUMA-----------------------------\n");
  410. #endif
  411. #ifdef STAN
  412.     printf("---STANFORD-ARM---------------------\n");
  413. #endif
  414.  
  415.     if ((scanf("%f%f%f%f%f%f%f%s"
  416.        , &x, &y, &z, &kx, &ky, &kz, &theta, conf)) == -1)
  417.         exit(0);
  418.     k.x = kx;
  419.     k.y = ky;
  420.     k.z = kz;
  421.     Rot(t, &k, theta);
  422.     Trsl(t, x, y, z);
  423.     return(conf);
  424. }
  425.  
  426.  
  427.  
  428. printj(j)
  429. JNTS_PTR j;
  430. {
  431.     double a[6], r[6];
  432.  
  433.     if (sframp) {
  434.         r[0] = j->th1;
  435.         r[1] = j->th2;
  436.         r[2] = j->th3;
  437.         r[3] = j->th4;
  438.         r[4] = j->th5;
  439.         r[5] = j->th6;
  440.         rngtoang(a, r);
  441.     }
  442.     else {
  443.         a[0] = j->th1;
  444.         a[1] = j->th2;
  445.         a[2] = j->th3;
  446.         a[3] = j->th4;
  447.         a[4] = j->th5;
  448.         a[5] = j->th6;
  449.     }
  450.  
  451.     printf("%8.3f %8.3f %8.3f %8.3f %8.3f %8.3f  %s %s\n",
  452.     a[0] * rdtodg_m,
  453.     a[1] * rdtodg_m,
  454. #ifdef PUMA
  455.     a[2] * rdtodg_m,
  456. #endif
  457. #ifdef STAN
  458.     a[2],
  459. #endif
  460.     a[3] * rdtodg_m,
  461.     a[4] * rdtodg_m,
  462.     a[5] * rdtodg_m,
  463.     j->conf,
  464.     (sframp) ? "   (sol ang)" : "   (rng ang)");
  465. }
  466.  
  467.  
  468. printsc()
  469. {
  470. #ifdef PUMA
  471.     if (fabs((sncs_d.s1 * sncs_d.s1 + sncs_d.c1 * sncs_d.c1) - 1) > SMALL) {
  472.         printf("J1 : bad sin cos------------------------\n");
  473.     }
  474.     if (fabs((sncs_d.s2 * sncs_d.s2 + sncs_d.c2 * sncs_d.c2) - 1) > SMALL) {
  475.         printf("J2 : bad sin cos------------------------\n");
  476.     }
  477.     if (fabs((sncs_d.s23 * sncs_d.s23 + sncs_d.c23 * sncs_d.c23) - 1) > SMALL) {
  478.         printf("J23 : bad sin cos------------------------\n");
  479.     }
  480.     if (fabs((sncs_d.s3 * sncs_d.s3 + sncs_d.c3 * sncs_d.c3) - 1) > SMALL) {
  481.         printf("J3 : bad sin cos------------------------\n");
  482.     }
  483.     if (fabs((sncs_d.s4 * sncs_d.s4 + sncs_d.c4 * sncs_d.c4) - 1) > SMALL) {
  484.         printf("J4 : bad sin cos------------------------\n");
  485.     }
  486.     if (fabs((sncs_d.s5 * sncs_d.s5 + sncs_d.c5 * sncs_d.c5) - 1) > SMALL) {
  487.         printf("J5 : bad sin cos------------------------\n");
  488.     }
  489.     if (fabs((sncs_d.s6 * sncs_d.s6 + sncs_d.c6 * sncs_d.c6) - 1) > SMALL) {
  490.         printf("J6 : bad sin cos------------------------\n");
  491.     }
  492. #endif
  493. #ifdef STAN
  494.     if (fabs((sncs_d.s1 * sncs_d.s1 + sncs_d.c1 * sncs_d.c1) - 1) > SMALL) {
  495.         printf("J1 : bad sin cos------------------------\n");
  496.     }
  497.     if (fabs((sncs_d.s2 * sncs_d.s2 + sncs_d.c2 * sncs_d.c2) - 1) > SMALL) {
  498.         printf("J2 : bad sin cos------------------------\n");
  499.     }
  500.     if (fabs((sncs_d.s4 * sncs_d.s4 + sncs_d.c4 * sncs_d.c4) - 1) > SMALL) {
  501.         printf("J4 : bad sin cos------------------------\n");
  502.     }
  503.     if (fabs((sncs_d.s5 * sncs_d.s5 + sncs_d.c5 * sncs_d.c5) - 1) > SMALL) {
  504.         printf("J5 : bad sin cos------------------------\n");
  505.     }
  506.     if (fabs((sncs_d.s6 * sncs_d.s6 + sncs_d.c6 * sncs_d.c6) - 1) > SMALL) {
  507.         printf("J6 : bad sin cos------------------------\n");
  508.     }
  509. #endif
  510. }
  511.  
  512.  
  513. diffdiff(r, a, b)
  514. DIFF_PTR r, a, b;
  515. {
  516.     r->t.x = a->t.x - b->t.x;
  517.     r->t.y = a->t.y - b->t.y;
  518.     r->t.z = a->t.z - b->t.z;
  519.     r->r.x = a->r.x - b->r.x;
  520.     r->r.y = a->r.y - b->r.y;
  521.     r->r.z = a->r.z - b->r.z;
  522. }
  523.  
  524.  
  525. printt(d)
  526. JNTS_PTR d;
  527. {
  528.     printf(
  529.     "1: %8.1e  2: %8.1e  3: %8.1e  4: %8.1e  5: %8.1e  6: %8.1e\n",
  530.     d->th1, d->th2, d->th3, d->th4, d->th5, d->th6);
  531. }
  532.