home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl040 < prev    next >
Text File  |  1987-03-02  |  27KB  |  1,101 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : setp.c
  7.  *      Remarks : This monster is the interrupt driven trajectory generator.
  8.  *                It maintains the time scheduling.
  9.  *                Each path segment it looks into the queue, pops out requests
  10.  *                and stores them in internal static variables.
  11.  *                Computes the delta motions for the transitions.
  12.  *                If moving coordinate frames, does something special point 2
  13.  *                of the transition.
  14.  *                Does also monitoring of forces and distances.
  15.  *                Most things happen at the first point of the transition.
  16.  *                Note that it is completely independant from the type of
  17.  *                arm it controls. Headache generator.
  18.  *      Usage   : part of the library
  19.  */
  20.  
  21. /*
  22.  * Et Dieu dit :
  23.  *
  24.  * T6 = COORD POS DRIVE COMPLY TOOL  (Cartesian)
  25.  *
  26.  * T6 = COORD POS COMPLY TOOL  (Joint int)
  27.  *
  28.  *      COORD, COMPLY, TOOL   : optional
  29.  */
  30.  
  31.  
  32. #include "../h/switch.h"
  33. #include "../h/rccl.h"
  34. #include "../h/manip.h"
  35. #include "../h/umac.h"
  36.  
  37. #define STOP_SEGT       10
  38. #define STOP_ACCT       5
  39. #ifndef REAL
  40. #define TR_to_JNS(j, t, c)      if(tr_to_jns_n(j, t, c)) {\
  41.                     giveup("joint(s) limit", NO);\
  42.                     return;}
  43. #else
  44. #define TR_to_JNS(j, t, c)      (void)tr_to_jns_n(j, t, c)
  45. #endif
  46.  
  47.  
  48. static TRFN monfn = (TRFN)NULL; /* pointer to the user's monitor function*/
  49.  
  50.  
  51.  
  52.  
  53.  
  54. setpoint_n() /*::*/
  55. {
  56.     /*
  57.      * static vars
  58.      */
  59.  
  60.     static JNTS jba,                        /* jb - ja              */
  61.             jbc,                        /* jb - jc              */
  62.             jabc,                       /* poly term            */
  63.             jba2,                       /* .........            */
  64.             ja,                         /* joints in A          */
  65.             jb,                         /* joints in B          */
  66.             jc = {"           "},       /* goal joints stores conf */
  67.             jo;                         /* last joints          */
  68.  
  69.     static DRVP drvbc,                      /* drive par b - c      */
  70.             drvba,                      /* ....  ... b - a      */
  71.             drabc,                      /* poly term            */
  72.             drba2,                      /* .... ....            */
  73.             drvu,                       /* differential drive par */
  74.             drvv;                       /* ............ ..... ... */
  75.  
  76.                         /* canonical equation terms */
  77.     static TRSF t6b  = {"T6B", const},      /* T6 in B              */
  78.             t6v  = {"T6V", const},      /* T6 when moving frames*/
  79.             t6o  = {"T6O", const},      /* last T6              */
  80.             posa = {"POSA", const},     /* POS in A             */
  81.             posb = {"POSB", const},     /* POS in B             */
  82.             posc = {"POSC", const},     /* POS in C             */
  83.             poso = {"POSO", const},     /* last POS             */
  84.             posv = {"POSV", const},     /* POS when moving frames */
  85.             coor = {"COOR", const},     /* COORD part           */
  86.             tool = {"TOOL", const},     /* TOOL part            */
  87.             coin = {"COIN", const},     /* COORD inverse        */
  88.             toin = {"TOIN", const},     /* TOOL inverse         */
  89.             cooo = {"COOO", const},     /* last COORD           */
  90.             tooo = {"TOOO", const},     /* last TOOL            */
  91.             drive= {"DRIVE", const},    /* current DRIVE        */
  92.             dist = {"DIST", const},     /* distance modifier    */
  93.             comply = {"COMPLY", const}; /* COMPLY accomodation  */
  94.  
  95.     static  int lastmode = '?',             /* mode last motion     */
  96.             currmode = '?',             /* mode of this motion  */
  97.             errset = OK,                /* tells in error state */
  98.             code = OK;                  /* current status       */
  99.  
  100.     static  int segmentime = 0,             /* current time segment */
  101.             tsg = 0,                    /* -ta1 -> segmentime - ta2 */
  102.             ta1 = 0,                    /* transition time      */
  103.             ta2 = 0;                    /* .......... .... of path */
  104.  
  105.     static bool armbefa = NO,               /* last point of path   */
  106.             armina = YES,               /* first point of path  */
  107.             armafta = NO;               /* second point of path */
  108.  
  109.     static real fmassobj;                   /* current mass of object */
  110.     static JNTS tobj;                       /* corresponding torques  */
  111.  
  112.     static int  forsel;                     /* force selection word */
  113.     static FORCE limfor;                    /* limit values         */
  114.  
  115.     static int cpysel;                      /* comply selection word */
  116.     static FORCE cpyfor;                    /* forces values        */
  117.  
  118.     static int difsel;                      /* diff motion sel. word */
  119.     static DIFF difmax;                     /* limit values         */
  120.  
  121.     static int exdsel;                      /* distance selection   */
  122.  
  123.     static bool lastfns = NO,               /* last motion had functions*/
  124.             nowfns = NO;                /* current motion has ...*/
  125.  
  126.     static real trat;                       /* term of poly         */
  127.  
  128.     static TERM_PTR updt = NULL;            /* term to update       */
  129.  
  130.     /*
  131.      * dynamic vars
  132.      */
  133.  
  134.     PST_PTR lsp = (PST_PTR)lastpos,         /* make casting now     */
  135.         glp = (PST_PTR)goalpos;
  136.     DRVP drvh;                              /* temp drive pars      */
  137.     real transvel, rotvel;                  /* for computing seg time */
  138.     TRSF temp;                              /* temp transform       */
  139.     bool forcedtrans = NO;                  /* YES when motion int. */
  140.     int newsmpl = 0;                        /* sample change        */
  141.     JNTS jerr;                              /* joint pos err        */
  142.     JNTS torques;                           /* to apply in comply m */
  143.     JNTS obsj;                              /* observed joint pos   */
  144.     DIFF tlerr,                             /* pos err in tool frame */
  145.          t6err;                             /* ... ... in t6 frame  */
  146.     int cpyjs = 0;                          /* comply joint selection */
  147.  
  148.     char *strcpy();
  149.  
  150. #ifdef FAKE
  151.     fprintf(stderr, "%d\n", rtime);         /* this can be removed  */
  152. #endif
  153.  
  154.     if (force_ctl) {                /* force stuff active   */
  155.         getobsj_n(&obsj);       /* get observed pos     */
  156.     }
  157. /*
  158.  * CALL USER MONITOR FUNCTION
  159.  */
  160.     if (monfn != (TRFN)NULL && tsg > -ta1 + 1) {
  161.         (* monfn)();                            /* call user's  */
  162.         if (terminate) {                        /* if user set it*/
  163.             return;
  164.         }
  165.     }
  166.  
  167. /*
  168.  * UPDATE GRAVITY LOAD DUE TO OBJECT MASS
  169.  *
  170.  * do it anyway to get U5 updated
  171.  */
  172.     {
  173.  
  174.         FORCE fg6;      /* gravity in link 6    */
  175.  
  176.         static FORCE forgrav = {0., 0., 0., 0., 0., 0.};
  177.         static TRSF y = {"Y", const,
  178.             1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.};
  179.  
  180.         forgrav.f.z = -fmassobj;
  181.         Takerot(&y, t6);
  182.         if (glp->toolp) {
  183.             Trmultinp(&y, &tool);
  184.         }
  185.         Forcetr(&fg6, &forgrav, &y);
  186.         jacobT_n(&tobj, &fg6);
  187.     }
  188.  
  189. /*
  190.  * DO FORCE STOP
  191.  */
  192.     if (forsel) {
  193.         FORCE ft6;                      /* force in link 6      */
  194.         JNTS jtor;                      /* corresp. joint torques */
  195.         JNTS jter;                      /* observed ..... ....... */
  196.         real n1, n2, n3, n4, n5, n6, n, d;
  197.  
  198.         if (glp->toolp) {
  199.             Forcetr(&ft6, &limfor, &tool);
  200.         }
  201.         else {
  202.             Assignforce(&ft6, &limfor);
  203.         }
  204.         jacobT_n(&jtor, &ft6);
  205.         getobst_n(&jter);
  206.  
  207.         /*               T        T
  208.          *      compute t  err / t  t
  209.          */
  210.  
  211.         d = jtor.th1 * jtor.th1 + jtor.th2 * jtor.th2 +
  212.             jtor.th3 * jtor.th3 + jtor.th4 * jtor.th4 +
  213.             jtor.th5 * jtor.th5 + jtor.th6 * jtor.th6;
  214.  
  215.         jter.th1 -= tobj.th1;
  216.         jter.th2 -= tobj.th2;
  217.         jter.th3 -= tobj.th3;
  218.         jter.th4 -= tobj.th4;
  219.         jter.th5 -= tobj.th5;
  220.         jter.th6 -= tobj.th6;
  221.         n1 = jtor.th1 * jter.th1;
  222.         n2 = jtor.th2 * jter.th2;
  223.         n3 = jtor.th3 * jter.th3;
  224.         n4 = jtor.th4 * jter.th4;
  225.         n5 = jtor.th5 * jter.th5;
  226.         n6 = jtor.th6 * jter.th6;
  227.  
  228.         n = FABS(n1) + FABS(n2) + FABS(n3)
  229.           + FABS(n4) + FABS(n5) + FABS(n6);
  230.         if (n > d) {
  231.             nextmove = ONF;         /* force transition     */
  232.         }
  233.     }
  234.  
  235.  
  236. /*
  237.  * Compute Cart changes if neccessary   (comply or diff motion stop)
  238.  */
  239.     if (difsel || cpysel) {
  240.         diffjnts_n(&jerr, &obsj, j6);
  241.         jacobD_n(&t6err, &jerr);
  242.         if (glp->toolp) {
  243.             Difftr(&tlerr, &t6err, &tool);
  244.         }
  245.         else {
  246.             Assigndiff(&tlerr, &t6err);
  247.         }
  248.     }
  249.  
  250. /*
  251.  * DO STOP DIST
  252.  */
  253.  
  254.     if ((difsel & SELTX) && FABS(tlerr.t.x) > difmax.t.x) {
  255.         nextmove = OND;
  256.     }
  257.     if ((difsel & SELTY) && FABS(tlerr.t.y) > difmax.t.y) {
  258.         nextmove = OND;
  259.     }
  260.     if ((difsel & SELTZ) && FABS(tlerr.t.z) > difmax.t.z) {
  261.         nextmove = OND;
  262.     }
  263.     if ((difsel & SELRX) && FABS(tlerr.r.x) > difmax.r.x) {
  264.         nextmove = OND;
  265.     }
  266.     if ((difsel & SELRY) && FABS(tlerr.r.y) > difmax.r.y) {
  267.         nextmove = OND;
  268.     }
  269.     if ((difsel & SELRZ) && FABS(tlerr.r.z) > difmax.r.z) {
  270.         nextmove = OND;
  271.     }
  272.  
  273.  
  274. /*
  275.  * COMPUTE COMPLY
  276.  */
  277.     if (cpysel) {
  278.         TRSF deltacpy;                  /* delta comply */
  279.  
  280.         /*
  281.          * set the errors in unwanted directions to zero
  282.          */
  283.  
  284.         if (!(cpysel & SELFX)) {
  285.             tlerr.t.x = 0.;
  286.         }
  287.         if (!(cpysel & SELFY)) {
  288.             tlerr.t.y = 0.;
  289.         }
  290.         if (!(cpysel & SELFZ)) {
  291.             tlerr.t.z = 0.;
  292.         }
  293.         if (!(cpysel & SELMX)) {
  294.             tlerr.r.x = 0.;
  295.         }
  296.         if (!(cpysel & SELMY)) {
  297.             tlerr.r.y = 0.;
  298.         }
  299.         if (!(cpysel & SELMZ)) {
  300.             tlerr.r.z = 0.;
  301.         }
  302.         Df_to_tr(&deltacpy, &tlerr);
  303.         Trmultinp(&comply, &deltacpy);          /* cummulate    */
  304.     }
  305.  
  306.  
  307. /*
  308.  * SEE IF NEXTMOVE SET, INCREMENT SEGMENT TIME
  309.  *
  310.  * tsg varies from (-ta1) to (segmentime - ta2) unless interrupted
  311.  */
  312.  
  313.     armbefa = tsg == segmentime - ta2 - 1;  /* compute 'before A' pred  */
  314.  
  315.     if (nextmove) {                         /* interrupted  */
  316.         armina = forcedtrans = YES;
  317.         segmentime = tsg + ta2;
  318.         code = nextmove;
  319.         nextmove = NO;
  320.     }
  321.     if (cpysel) {
  322.         forcedtrans = YES;              /* no func. trans. smooth */
  323.     }
  324.     ++tsg;
  325.  
  326. /*
  327.  * FIRST POINT OF TRANSITION, GET NEW INFO FROM QUEUE, STOP IF NOTHING
  328.  */
  329.     if (armina) {
  330.         static int stopped = NO;
  331.         MOT_PTR newm;                   /* new motion record    */
  332.  
  333.         Assigntr(here, t6);             /* update here, user trans */
  334.         if (updt != NULL) {
  335.             if (updt->rhs) {
  336.                 solvei_n(updt->altr, updt, updt);
  337.             }
  338.             else {
  339.                 solved_n(updt->altr, updt, updt);
  340.             }
  341.         }
  342.         lastpos = goalpos;
  343.         lsp = glp;
  344.         Assigntr(&poso, &posc);
  345.         goalpos->code = code;
  346.         if (!stopped) {
  347.             --(goalpos->end);       /* signal event         */
  348.         }
  349.         if (code == LIMIT) {            /* force move to there  */
  350.             glp = (PST_PTR)(goalpos = there);
  351.             Assigntr(&posc, here);
  352.             newm = NULL;
  353.         }
  354.         else {
  355.             /*
  356.              * get a new motion record
  357.              */
  358.  
  359.             newm = (MOT_PTR)dequeue_n(&mqueue_n);
  360.  
  361.             if (newm == NULL) {             /* queue empty  */
  362.                 if (completed > 0) {
  363.                     --completed;    /* signal event */
  364.                 }
  365.                 stopped = YES;          /* stop ==      */
  366.             }
  367.             else {
  368.                 --requestnb;
  369.                 stopped = NO;
  370.             }
  371.         }
  372.         if (newm == NULL) {
  373.             if (lastmode == '?') {  /* to get things started */
  374.                 ta2 = STOP_ACCT;
  375.                 tsg = 0;
  376.             }
  377.             ta1 = ta2;              /* make the shortest path */
  378.             tsg -= segmentime;      /* as possible          */
  379.             segmentime = ta2 + ta2;
  380.             if (code != OK) {       /* do no transition     */
  381.                 ta1 = 0;
  382.             }
  383.         }
  384.         else {
  385.             if (newm->pst == NULL) {        /* user's stop  */
  386.                 ta1 = ta2;
  387.                 tsg -= segmentime;
  388.                 segmentime = ta2 + ta2 + newm->sgt;
  389.             }
  390.             else {                          /* normal case  */
  391.                 /*
  392.                  * get all the new parameters
  393.                  */
  394.  
  395.                 TERM_PTR tp;
  396.                 TRSF_PTR trp;
  397.  
  398.                 if (lastmode == '?') {
  399.                     ta2 = ta1 = newm->acct;
  400.                     tsg = -ta2;
  401.                 }
  402.                 else {
  403.                     ta1 = ta2;
  404.                     ta2 = newm->acct;
  405.                     tsg -= segmentime;
  406.                 }
  407.  
  408.                 segmentime = newm->sgt;
  409.                 if (segmentime == 0) {          /* use vel. */
  410.                     transvel = newm->velt;
  411.                     rotvel = newm->velr;
  412.                 }
  413.                 lastmode = currmode;
  414.                 goalpos = (POS_PTR)(glp = newm->pst);
  415.                 currmode = newm->mod;
  416.              (void) strcpy(jc.conf, newm->cfg);
  417.                 monfn = newm->fnt;
  418.                 updt = newm->upd;
  419.  
  420.                 if ((newsmpl = newm->smpl) != 0) {
  421.                     real fact, ftim;
  422.  
  423.                     fact = (real)timeincrement / (real)newsmpl;
  424.                     ftim = tsg;
  425.                     ftim *= fact;
  426.                     tsg = ftim;
  427.                     ftim = ta1;
  428.                     ftim *= fact;
  429.                     ta1 = ftim;
  430.                     timeincrement = newsmpl;
  431.                     if (ta1 < 2) {
  432.                         ta1 = 2;
  433.                     }
  434.                 }
  435.                 fmassobj = newm->fmass;
  436.                 forsel = newm->fsp;
  437.                 Assignforce(&limfor, &(newm->frc));
  438.                 cpysel = newm->csp;
  439.                 Assignforce(&cpyfor, &(newm->cpy));
  440.                 if (!force_ctl) {
  441.                     cpysel = 0;
  442.                     forsel = 0;
  443.                 }
  444.                 if (!cpysel) {
  445.                     Assigntr(&comply, unitr);
  446.                 }
  447.                 difsel = newm->dsp;
  448.                 Assigndiff(&difmax, &(newm->dst));
  449.  
  450.                 /*
  451.                  * get the hold transforms
  452.                  */
  453.                 for (tp = glp->t6ptr->prev;
  454.                      tp != glp->t6ptr;
  455.                      tp = tp->prev) {
  456.                     if (tp->trsf->fn == hold) {
  457.                         if ((trp = (TRSF_PTR)
  458.                             dequeue_n(&(tp->hd)))
  459.                               == NULL) {
  460.                             giveup("jam", NO);
  461.                             return;
  462.                         }
  463.                         Assigntr(tp->altr, trp);
  464.                     }
  465.                 }
  466.                 exdsel = newm->exp;
  467.                 if (exdsel) {
  468.                     Df_to_tr(&dist, &(newm->exd));
  469.                     Trmult(&posc, glp->pos->altr, &dist);
  470.                 }
  471.                 else {
  472.                     Assigntr(&dist, unitr);
  473.                     Assigntr(&posc, glp->pos->altr);
  474.                 }
  475.             }
  476.         }
  477.     }
  478.     rtime += timeincrement;         /* time is passing              */
  479.  
  480. /*
  481.  * COMPUTE DELTAS
  482.  */
  483.  
  484. /*
  485.  * perform some updates, compute scal
  486.  */
  487.     if (armina) {
  488.         lastfns = nowfns;
  489.         nowfns = glp->cfnsp || glp->tfnsp;
  490.         if (lastmode == '?') {
  491.             jns_to_tr_n(t6, j6, NO);
  492.             Assigntr(&t6b, t6);
  493.             assignjs_n(&jb, j6);
  494.             assignjs_n(&jc, j6);
  495.          (void) strcpy(jc.conf, j6->conf);
  496.             shifttr_n(glp);
  497.         }
  498.         goalpos->scal = 0.;
  499.     }
  500.     else {
  501.         goalpos->scal = (real)(tsg + ta1 - 1) /
  502.                 (real)(segmentime - ta2 + ta1);
  503.     }
  504.  
  505. /*
  506.  * case with no transition (ta1 null)
  507.  */
  508.     if (armina && ta1 == 0) {
  509.         switch (currmode) {
  510.         case 'c' :
  511.             /*
  512.              * just have to compute posb
  513.              */
  514.  
  515.             if (glp->coorp) {
  516.                 solvei_n(&coin, glp->t6ptr, glp->pos);
  517.                 Trmult(&posb, &coin, t6);
  518.             }
  519.             else {
  520.                 Assigntr(&posb, t6);
  521.             }
  522.             if (glp->toolp) {
  523.                 solvei_n(&toin, glp->pos, glp->t6ptr);
  524.                 Trmultinp(&posb, &toin);
  525.             }
  526.             if (cpysel) {
  527.                 Trmultinv(&posb, &comply);
  528.             }
  529.             if (terminate) {
  530.                 return;
  531.             }
  532.             setpar_n(&drvbc, &posb, &posc);
  533.             break;
  534.  
  535.         case 'j' :
  536.             /*
  537.              * get the canon. equation terms anyhow because
  538.              * tool part is needed
  539.              */
  540.  
  541.             assignjs_n(&jb, j6);
  542.             if (glp->coorp) {
  543.                 solved_n(&coor, glp->t6ptr, glp->pos);
  544.                 Trmult(&t6b, &coor, glp->pos->altr);
  545.             }
  546.             else {
  547.                 Assigntr(&t6b, glp->pos->altr);
  548.             }
  549.             if (cpysel) {
  550.                 Trmultinp(&t6b, &comply);
  551.             }
  552.             if (glp->toolp) {
  553.                 solved_n(&tool, glp->pos, glp->t6ptr);
  554.                 Trmultinp(&t6b, &tool);
  555.             }
  556.             TR_to_JNS(&jc, &t6b, NO);
  557.             diffjnts_n(&jbc, &jc, &jb);
  558.             break;
  559.         }
  560.     }
  561.  
  562.  
  563. /*
  564.  * normal case
  565.  */
  566.     if (armina && ta1 != 0) {
  567.         switch (currmode) {
  568.         case 'c' :
  569.             if (lastmode != 'c' || forcedtrans) {
  570.  
  571.                 /*
  572.                  * extrapolate t6b
  573.                  */
  574.  
  575.                 setpar_n(&drvh, &t6o, t6);
  576.                 fopar_n(&drvh, &drvh,
  577.                     (DRVP_PTR)NULL, (real)ta1);
  578.                 drivefn_n(&drive, &drvh);
  579.                 Trmult(&t6b, t6, &drive);
  580.             }
  581.             else {
  582.                 /*
  583.                  * compute t6b with old equation
  584.                  * redo the computations only if moving
  585.                  * frames are involved
  586.                  */
  587.  
  588.                 POS_PTR savepos;
  589.  
  590.                 savepos = goalpos;      /* cheat to get */
  591.                 goalpos = lastpos;      /* to get scal  */
  592.                 goalpos->scal = 1.;     /* 0 ... .99 1. */
  593.  
  594.                 if (lsp->coorp && lsp->cfnsp) {
  595.                     solvedo_n(&cooo, lsp->t6ptr, lsp->pos);
  596.                 }
  597.                 if (lsp->toolp && lsp->tfnsp) {
  598.                     solvedo_n(&tooo, lsp->pos, lsp->t6ptr);
  599.                 }
  600.                 if (lsp->pos->rhs) {
  601.                     Assigntr(&temp, &poso);
  602.                 }
  603.                 else {
  604.                     Invert(&temp, &poso);
  605.                 }
  606.                 if (lsp->coorp) {
  607.                     Trmult(&t6b, &cooo, &temp);
  608.                 }
  609.                 else {
  610.                     Assigntr(&t6b, &temp);
  611.                 }
  612.                 if (lsp->toolp) {
  613.                     Trmultinp(&t6b, &tooo);
  614.                 }
  615.  
  616.                 if (lastfns) {
  617.  
  618.                     /*
  619.                      * we need to compute t6 with old
  620.                      * equation that has moved meanwhile
  621.                      * coord and tool are ready
  622.                      */
  623.  
  624.                     if (lsp->coorp) {
  625.                         Trmult(t6, &cooo, &posb);
  626.                     }
  627.                     else {
  628.                         Assigntr(t6, &posb);
  629.                     }
  630.                     Trmultinp(t6, &drive);
  631.                     if (lsp->toolp) {
  632.                         Trmultinp(t6, &tooo);
  633.                     }
  634.                 }
  635.                 goalpos = savepos;
  636.             }
  637.  
  638.             /*
  639.              * compute posa posb from canon. equation
  640.              */
  641.  
  642.             if (glp->coorp) {
  643.                 solvei_n(&coin, glp->t6ptr, glp->pos);
  644.                 Trmult(&posa, &coin, t6);
  645.                 Trmult(&posb, &coin, &t6b);
  646.             }
  647.             else {
  648.                 Assigntr(&posa, t6);
  649.                 Assigntr(&posb, &t6b);
  650.             }
  651.             if (glp->toolp) {
  652.                 solvei_n(&toin, glp->pos, glp->t6ptr);
  653.                 Trmultinp(&posa, &toin);
  654.                 Trmultinp(&posb, &toin);
  655.             }
  656.             if (cpysel) {
  657.                 Invert(&temp, &comply);
  658.                 Trmultinp(&posa, &temp);
  659.                 Trmultinp(&posb, &temp);
  660.             }
  661.             if (terminate) {
  662.                 return;
  663.             }
  664.             setpar_n(&drvba, &posb, &posa);
  665.             setpar_n(&drvbc, &posb, &posc);
  666.  
  667.             /*
  668.              * compute corrective terms first point of transition
  669.              * only in the case of continuous cart mode and
  670.              * no forced trans and last motion had moving frames
  671.              */
  672.  
  673.             drvv.dx = drvv.dy = drvv.dz =
  674.             drvv.dphi = drvv.dthe = 0.;
  675.             if (lastmode == 'c' && lastfns && !forcedtrans) {
  676.                 if (glp->coorp) {
  677.                     Trmult(&posv, &coin, &t6v);
  678.                 }
  679.                 else {
  680.                     Assigntr(&posv, &t6v);
  681.                 }
  682.                 if (glp->toolp) {
  683.                     Trmultinp(&posv, &toin);
  684.                 }
  685.                 setpar_n(&drvu, &posv, &posb);
  686.             }
  687.             else {
  688.                 drvu.dx = drvu.dy = drvu.dz =
  689.                 drvu.dphi = drvu.dthe = 0.;
  690.             }
  691.  
  692.             /*
  693.              * adjust psi
  694.              */
  695.  
  696.             if (drvbc.dpsi - drvba.dpsi > pib2_m) {
  697.                 drvba.dpsi += pi_m ;
  698.                 drvba.dthe = - drvba.dthe;
  699.             }
  700.             if (drvba.dpsi - drvbc.dpsi > pib2_m) {
  701.                 drvba.dpsi -= pi_m;
  702.                 drvba.dthe = - drvba.dthe;
  703.             }
  704.             break;
  705.  
  706.         case 'j' :
  707.  
  708.             /*
  709.              * if not continuous joint mode extrapolate pos jB
  710.              */
  711.  
  712.             if (lastmode != 'j' || forcedtrans) {
  713.                 fojnts_n(&jb, jd, j6, (real)ta1);
  714.             }
  715.             else {
  716.                 assignjs_n(&jb, &jc);
  717.             }
  718.             assignjs_n(&ja, j6);
  719.             if (glp->coorp) {
  720.                 solved_n(&coor, glp->t6ptr, glp->pos);
  721.                 Trmult(&t6b, &coor, &posc);
  722.             }
  723.             else {
  724.                 Assigntr(&t6b, &posc);
  725.             }
  726.             if (cpysel) {
  727.                 Trmultinp(&t6b, &comply);
  728.             }
  729.             if (glp->toolp) {
  730.                 solved_n(&tool, glp->pos, glp->t6ptr);
  731.                 Trmultinp(&t6b, &tool);
  732.             }
  733.             TR_to_JNS(&jc, &t6b, NO);
  734.             diffjnts_n(&jba, &ja, &jb);
  735.             diffjnts_n(&jbc, &jc, &jb);
  736.             break;
  737.         }
  738.     }
  739.  
  740. /*
  741.  * COMPUTE SEGMENT TIME IF VELOCITY SPECIFIED AND SOME COEF VALID FOR ONE PATH
  742.  */
  743.     if (armina) {
  744.         if (segmentime == 0) {
  745.             real sg1, sg2, sgtm;
  746.  
  747.             if (currmode == 'j') {
  748.                 setpar_n(&drvbc, t6, &t6b);
  749.             }
  750.             sg1 = sqrt(drvbc.dx * drvbc.dx
  751.                  + drvbc.dy * drvbc.dy
  752.                  + drvbc.dz * drvbc.dz)
  753.                  / transvel;
  754.             sg2 = sqrt(drvbc.dphi * drvbc.dphi
  755.                  + drvbc.dthe * drvbc.dthe) * rdtodg_m
  756.                  / rotvel;
  757.             sgtm = ((sg1 > sg2) ? sg1 : sg2) * 1000. / timeincrement;
  758.             segmentime = sgtm + ta2 + ta2;
  759.             if (segmentime < STOP_SEGT) {
  760.                 segmentime = STOP_SEGT;
  761.             }
  762.         }
  763.  
  764.         /*
  765.          * compute terms of the polynomials
  766.          */
  767.  
  768.         if (ta1 !=  0) {
  769.             trat = (real)ta1 / (real)segmentime;
  770.             switch (currmode) {
  771.             case 'c' :
  772.                 fopar_n(&drabc, &drvbc, &drvba, trat);
  773.                 t2par_n(&drba2, &drvba);
  774.                 break;
  775.  
  776.             case 'j' :
  777.                 fojnts_n(&jabc, &jbc, &jba, trat);
  778.                 t2jnts_n(&jba2, &jba);
  779.                 break;
  780.             }
  781.         }
  782.     }
  783.  
  784. /*
  785.  * ALLOW SOME PRINTS FOR DEBUG VERSIONS
  786.  */
  787.  
  788. #ifndef REAL
  789.     if (prints_out && armina) {
  790.         DIFF exdist;
  791.  
  792.         fprintf(fpi, "%s %d %d %c %d %d %d %d\n",
  793.             goalpos->name,
  794.             lastpos->code,
  795.             rtime,
  796.             currmode,
  797.             ta1 * timeincrement,
  798.             ta2 * timeincrement,
  799.             segmentime * timeincrement,
  800.             timeincrement);
  801.         fprintf(fpi, "\tforce 0%o %g %g %g %g %g %g\n",
  802.             forsel,
  803.             limfor.f.x, limfor.f.y, limfor.f.z,
  804.             limfor.m.x, limfor.m.y, limfor.m.z);
  805.         fprintf(fpi, "\tcply  0%o %g %g %g %g %g %g\n",
  806.             cpysel,
  807.             cpyfor.f.x, cpyfor.f.y, cpyfor.f.z,
  808.             cpyfor.m.x, cpyfor.m.y, cpyfor.m.z);
  809.         fprintf(fpi, "\tdst   0%o %g %g %g %g %g %g\n",
  810.             difsel,
  811.             difmax.t.x, difmax.t.y, difmax.t.z,
  812.             difmax.r.x, difmax.r.y, difmax.r.z);
  813.         Tr_to_df(&exdist, &dist);
  814.         fprintf(fpi, "\texd   0%o %g %g %g %g %g %g\n",
  815.             exdsel,
  816.             exdist.t.x, exdist.t.y, exdist.t.z,
  817.             exdist.r.x, exdist.r.y, exdist.r.z);
  818.     }
  819. #endif
  820.  
  821.  
  822. /*
  823.  * COMPUTE CORRECTIONS SECOND POINT OF TRANSITION
  824.  *
  825.  * do that only if the path has moving frames and there is a transition
  826.  */
  827.     if (armafta && nowfns && ta1 != 0) {
  828.         switch (currmode) {
  829.         case 'c' :
  830.  
  831.             /*
  832.              * compute the differential POS change
  833.              */
  834.  
  835.             if (glp->coorp) {
  836.                 if (glp->cfnsp) {
  837.                     solvei_n(&coin, glp->t6ptr, glp->pos);
  838.                 }
  839.                 Trmult(&posv, &coin, &t6b);
  840.             }
  841.             else {
  842.                 Assigntr(&posv, &t6b);
  843.             }
  844.             if (glp->toolp) {
  845.                 if (glp->tfnsp) {
  846.                     solvei_n(&toin, glp->pos, glp->t6ptr);
  847.                 }
  848.                 Trmultinp(&posv, &toin);
  849.             }
  850.             if (cpysel) {
  851.                 Trmultinv(&posv, &comply);
  852.             }
  853.             setpar_n(&drvv, &posv, &posb);
  854.             break;
  855.  
  856.         case 'j' :
  857.  
  858.             /*
  859.              * compute the position we expect to be at the
  860.              * end of path in cart space
  861.              */
  862.  
  863.             if (glp->coorp) {
  864.                 solved_n(&coor, glp->t6ptr, glp->pos);
  865.                 Trmult(&t6v, &coor, &posc);
  866.             }
  867.             else {
  868.                 Assigntr(&t6v, &posc);
  869.             }
  870.             if (cpysel) {
  871.                 Trmultinp(&t6v, &comply);
  872.             }
  873.             if (glp->toolp) {
  874.                 solved_n(&tool, glp->pos, glp->t6ptr);
  875.                 Trmultinp(&t6v, &tool);
  876.             }
  877.             setpar_n(&drvh, &t6b, &t6v);
  878.             fopar_n(&drvh, &drvh,
  879.             (DRVP_PTR)NULL, (real)segmentime);
  880.             drivefn_n(&drive, &drvh);
  881.             Trmultinp(&t6b, &drive);
  882.             TR_to_JNS(&jc, &t6b, NO);
  883.             diffjnts_n(&jbc, &jc, &jb);
  884.             fojnts_n(&jabc, &jbc, &jba, trat);
  885.             break;
  886.         }
  887.     }
  888.  
  889. /*
  890.  * SELECT COMPLIANT JOINT(S)
  891.  */
  892.     if (cpysel) {
  893.         if (glp->toolp) {
  894.             cpyjs = select_n(cpysel, &tool);
  895.         }
  896.         else {
  897.             cpyjs = select_n(cpysel, (TRSF_PTR)NULL);
  898.         }
  899.     }
  900.  
  901.  
  902. /*
  903.  * COMPUTE CURRENT POINT
  904.  */
  905.     Assigntr(&t6o, t6);
  906.     assignjs_n(&jo, j6);
  907.  
  908.     switch(currmode) {
  909.     real h, hm2, hv, hmv;
  910.  
  911.     case 'c' :
  912.         if (tsg < ta1 && ta1 != 0) {    /* transition   */
  913.             int tas;
  914.  
  915.             h = (real)(tsg + ta1) / (real)(ta1 + ta1);
  916.             hm2 = (2. - h) * h * h;
  917.             tas = ta1 - 1;
  918.             hv = (real)(tsg + tas) / (real)(tas + tas);
  919.             hmv = hv * (hv * (hv - 2.) + 1.) * (ta1 + ta1);
  920.             drvh.dpsi = (drvbc.dpsi - drvba.dpsi) * h
  921.                     + drvba.dpsi;
  922.  
  923.             polypar_n(&drvh, &drabc, hm2, &drba2, h, &drvba,
  924.                 &drvu, hmv, &drvv, -hmv);
  925.         }
  926.         else {                          /* straight      */
  927.             h = (real)tsg / (real)segmentime;
  928.             drvh.dpsi = drvbc.dpsi;
  929.             fopar_n(&drvh, &drvbc, (DRVP_PTR)NULL, h);
  930.         }
  931.         drivefn_n(&drive, &drvh);
  932.  
  933.         /*
  934.          * avoid here to redo computations that have been done
  935.          * when computing the deltas or the corrections
  936.          */
  937.  
  938.         if (!(armina || (armafta && nowfns)) || ta1 == 0) {
  939.             if (glp->coorp && glp->cfnsp) {
  940.                 solvei_n(&coin, glp->t6ptr, glp->pos);
  941.             }
  942.             if (glp->toolp && glp->tfnsp) {
  943.                 solvei_n(&toin, glp->pos, glp->t6ptr);
  944.             }
  945.         }
  946.  
  947.         /*
  948.          * compute t6 and j6
  949.          */
  950.  
  951.         if (glp->coorp) {
  952.             Invert(&coor, &coin);
  953.             Trmult(t6, &coor, &posb);
  954.         }
  955.         else {
  956.             Assigntr(t6, &posb);
  957.         }
  958.         Trmultinp(t6, &drive);
  959.  
  960.         if (cpysel) {
  961.             Trmultinp(t6, &comply);
  962.         }
  963.         if (glp->toolp) {
  964.             Invert(&tool, &toin);
  965.             Trmultinp(t6, &tool);
  966.         }
  967.         TR_to_JNS(j6, t6, YES);
  968.  
  969.         if (armbefa) {                  /* save the hold transforms */
  970.             shifttr_n(glp);         /* if regular transition    */
  971.         }                               /* will occur next time     */
  972.  
  973.         /*
  974.          * compute ahead coord and tool for next motion
  975.          * so it will not have to be done when computing deltas
  976.          */
  977.  
  978.         if (armbefa) {
  979.             if (glp->coorp) {
  980.                 solvedo_n(&cooo, glp->t6ptr, glp->pos);
  981.             }
  982.             if (glp->toolp) {
  983.                 solvedo_n(&tooo, glp->pos, glp->t6ptr);
  984.             }
  985.             if (glp->pos->rhs) {
  986.                 Assigntr(&temp, &posc);
  987.             }
  988.             else {
  989.                 Invert(&temp, &posc);
  990.             }
  991.             if (glp->coorp) {
  992.                 Trmult(&t6v, &cooo, &temp);
  993.             }
  994.             else {
  995.                 Assigntr(&t6v, &temp);
  996.             }
  997.             if (glp->toolp) {
  998.                 Trmultinp(&t6v, &tooo);
  999.             }
  1000.         }
  1001.         break;
  1002.  
  1003.     case 'j' :
  1004.  
  1005.         /*
  1006.          * in comply mode the poly coeff need to be ajusted
  1007.          * with the actual joint motions
  1008.          */
  1009.  
  1010.         if (tsg < ta1 && ta1 != 0) {            /* transition   */
  1011.             h = (real)(tsg + ta1) / (real)(2 * ta1);
  1012.             hm2 = (2. - h) * h * h;
  1013.             polycpyc_n(&ja, &jabc, &jba2, hm2, h, &obsj, cpyjs);
  1014.             polyjnts_n(j6, &jabc, &jba2, &ja, hm2, h);
  1015.         }
  1016.         else {                                  /* straight     */
  1017.             h = (real)tsg / (real)segmentime;
  1018.             focpyc_n(&jb, &jbc, h, &obsj, cpyjs);
  1019.             fojnts_n(j6, &jbc, &jb, h);
  1020.         }
  1021.  
  1022.         jns_to_tr_n(t6, j6, YES);       /* t6 still need to be  */
  1023.                         /* computed             */
  1024.         if (armbefa) {
  1025.             shifttr_n(glp);
  1026.         }
  1027.         break;
  1028.  
  1029.     case '?' :
  1030.         break;
  1031.     }
  1032.  
  1033.     diffjnts_n(jd, j6, &jo);                /* compute velocity     */
  1034.  
  1035.     if (cpyjs) {                            /* correct loads in comply */
  1036.         jacobT_n(&torques, &cpyfor);
  1037.         torques.th1 -= tobj.th1;
  1038.         torques.th2 -= tobj.th2;
  1039.         torques.th3 -= tobj.th3;
  1040.         torques.th4 -= tobj.th4;
  1041.         torques.th5 -= tobj.th5;
  1042.         torques.th6 -= tobj.th6;
  1043.     }
  1044.  
  1045. /*
  1046.  * SEND RESULTS
  1047.  */
  1048.  
  1049.  
  1050. #ifdef REAL
  1051.     code = jnsend_n(j6, newsmpl, cpyjs, &torques);
  1052. #else
  1053.     code = jnsend_n(glp, j6, &jo, t6, tsg * timeincrement, rtime, cpyjs,
  1054.            (currmode == 'c')
  1055.         ?       ((tsg < ta1)
  1056.             ?       ((armina)
  1057.                 ?       'V'
  1058.                 :       'H')
  1059.             :       'C')
  1060.         :       ((tsg < ta1) ?
  1061.                 ((armina)
  1062.                 ?       'E'
  1063.                 :       'T')
  1064.             :       'J')
  1065.         );
  1066. #endif
  1067.  
  1068. /*
  1069.  * CHECK RESULTS
  1070.  *
  1071.  * with errset, code is forced to OK
  1072.  */
  1073.     if (errset != OK || goalpos == there) {
  1074.         code = OK;
  1075.     }
  1076.     if (armbefa && goalpos != there) {
  1077.         errset = code;
  1078.     }
  1079.     if (code != OK) {
  1080.         errset = nextmove = code;
  1081.     }
  1082. /*
  1083.  * SHIFT PREDICATES
  1084.  */
  1085.     armafta = armina;
  1086.     armina = armbefa;
  1087.  
  1088.     /* that's all   */
  1089. }
  1090.  
  1091.  
  1092.  
  1093. checkstate_n() /*::*/
  1094. {
  1095. /*
  1096.  * the code here has been moved at the beginning of setpoint
  1097.  * because of driver problem.
  1098.  */
  1099. }
  1100.  
  1101.