home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL024 < prev    next >
Encoding:
Text File  |  1987-03-02  |  9.4 KB  |  488 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : select.c
  7.  *      Remarks : Perform a simple minded joint selection algorithm for comply
  8.  *                The method is to compare the elements of the jacobian
  9.  *                3 X 3 upper left matrix for translations
  10.  *                3 X 3 lower right ........  rotations
  11.  *                The tool transform is passed as argument, NULL is passed if
  12.  *                none, changes transformed into L4 coordinate.
  13.  *                Need some more sophisticated method.
  14.  *      Usage   : part of library
  15.  */
  16.  
  17. #include "../h/rccl.h"
  18. #include "../h/manip.h"
  19. #include "../h/which.h"
  20. #include "../h/kine.h"
  21. #include "../h/umac.h"
  22.  
  23.  
  24. /*
  25.  * Selects the joint most likely to provide for motion along or
  26.  * around principal directions of t6.
  27.  * assumes that 1 2 3 will do for positions and 4 5 6 for rotations.
  28.  * returns joints selection, one per direction.
  29.  * assumes coeff U5, jacob updated
  30.  */
  31.  
  32. #ifdef PUMA
  33.  
  34. select_n(dirs, tool) /*::*/
  35. register int dirs;
  36. register TRSF_PTR tool;
  37. {
  38.     register int best = 0;
  39.  
  40.     if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFY | SELFZ)) {
  41.         best |= SELJ1 | SELJ2 | SELJ3;
  42.     }
  43.     else {
  44.         real ct1x, ct1y, ct1z, ct2x, ct2y, ct2z, ct3x, ct3y, ct3z;
  45.         TRSF tot4;
  46.  
  47.         if (tool != NULL) {
  48.             Trmult(&tot4, &sncs_d.u5, tool);
  49.         }
  50.         else {
  51.             Assigntr(&tot4, &sncs_d.u5);
  52.         }
  53.  
  54.         if (dirs & SELFX) {
  55.             ct1x = tot4.n.x * sncs_d.d1x +
  56.                    tot4.n.y * sncs_d.d1y +
  57.                    tot4.n.z * sncs_d.d1z;
  58.             ct1x = FABS(ct1x);
  59.  
  60.             ct2x = tot4.n.x * sncs_d.d2x +
  61.                    tot4.n.y * sncs_d.d2y +
  62.                    tot4.n.z * sncs_d.d2z;
  63.  
  64.             ct3x = tot4.n.x * sncs_d.d3x +
  65.                    tot4.n.y * sncs_d.d3y +
  66.                    tot4.n.z * sncs_d.d3z;
  67.  
  68.             ct2x = ct3x + ct2x;
  69.             ct2x = FABS(ct2x);
  70.  
  71.             ct3x = FABS(ct3x);
  72.         }
  73.         if (dirs & SELFY) {
  74.             ct1y = tot4.o.x * sncs_d.d1x +
  75.                    tot4.o.y * sncs_d.d1y +
  76.                    tot4.o.z * sncs_d.d1z;
  77.             ct1y = FABS(ct1y);
  78.  
  79.             ct2y = tot4.o.x * sncs_d.d2x +
  80.                    tot4.o.y * sncs_d.d2y +
  81.                    tot4.o.z * sncs_d.d2z;
  82.  
  83.             ct3y = tot4.o.x * sncs_d.d3x +
  84.                    tot4.o.y * sncs_d.d3y +
  85.                    tot4.o.z * sncs_d.d3z;
  86.  
  87.             ct2y = ct2y + ct3y;
  88.             ct2y = FABS(ct2y);
  89.  
  90.             ct3y = FABS(ct3y);
  91.         }
  92.         if (dirs & SELFZ) {
  93.             ct1z = tot4.a.x * sncs_d.d1x +
  94.                    tot4.a.y * sncs_d.d1y;
  95.             ct1z = FABS(ct1z);
  96.  
  97.             ct2z = tot4.a.x * sncs_d.d2x +
  98.                    tot4.a.y * sncs_d.d2y;
  99.  
  100.             ct3z = tot4.a.x * sncs_d.d3x +
  101.                    tot4.a.y * sncs_d.d3y;
  102.  
  103.             ct2z = ct2z + ct3z;
  104.             ct2z = FABS(ct2z);
  105.  
  106.             ct3z = FABS(ct3z);
  107.         }
  108.  
  109.         if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFY)) {
  110.             if ((ct1x > ct2x) && (ct1x > ct3x)) {
  111.                 best |= SELJ1;
  112.                 if (ct2y > ct3y) {
  113.                     best |= SELJ2;
  114.                 }
  115.                 else {
  116.                     best |= SELJ3;
  117.                 }
  118.             }
  119.             else {
  120.                 if (ct2x > ct3x) {
  121.                     best |= SELJ2;
  122.                     if (ct1y > ct3y) {
  123.                         best |= SELJ1;
  124.                     }
  125.                     else {
  126.                         best |= SELJ3;
  127.                     }
  128.                 }
  129.                 else {
  130.                     best |= SELJ3;
  131.                     if (ct1y > ct2y) {
  132.                         best |= SELJ1;
  133.                     }
  134.                     else {
  135.                         best |= SELJ2;
  136.                     }
  137.                 }
  138.             }
  139.         }
  140.         if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFZ)) {
  141.             if ((ct1x > ct2x) && (ct1x > ct3x)) {
  142.                 best |= SELJ1;
  143.                 if (ct2z > ct3z) {
  144.                     best |= SELJ2;
  145.                 }
  146.                 else {
  147.                     best |= SELJ3;
  148.                 }
  149.             }
  150.             else {
  151.                 if (ct2x > ct3x) {
  152.                     best |= SELJ2;
  153.                     if (ct1z > ct3z) {
  154.                         best |= SELJ1;
  155.                     }
  156.                     else {
  157.                         best |= SELJ3;
  158.                     }
  159.                 }
  160.                 else {
  161.                     best |= SELJ3;
  162.                     if (ct1z > ct2z) {
  163.                         best |= SELJ1;
  164.                     }
  165.                     else {
  166.                         best |= SELJ2;
  167.                     }
  168.                 }
  169.             }
  170.         }
  171.         if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFY | SELFZ)) {
  172.             if ((ct1y > ct2y) && (ct1y > ct3y)) {
  173.                 best |= SELJ1;
  174.                 if (ct2z > ct3z) {
  175.                     best |= SELJ2;
  176.                 }
  177.                 else {
  178.                     best |= SELJ3;
  179.                 }
  180.             }
  181.             else {
  182.                 if (ct2y > ct3y) {
  183.                     best |= SELJ2;
  184.                     if (ct1z > ct3z) {
  185.                         best |= SELJ1;
  186.                     }
  187.                     else {
  188.                         best |= SELJ3;
  189.                     }
  190.                 }
  191.                 else {
  192.                     best |= SELJ3;
  193.                     if (ct1z > ct2z) {
  194.                         best |= SELJ1;
  195.                     }
  196.                     else {
  197.                         best |= SELJ2;
  198.                     }
  199.                 }
  200.             }
  201.         }
  202.         if ((dirs & (SELFX | SELFY | SELFZ)) == SELFX) {
  203.             if ((ct1x > ct2x) && (ct1x > ct3x)) {
  204.                 best |= SELJ1;
  205.             }
  206.             else {
  207.                 if (ct2x > ct3x) {
  208.                     best |= SELJ2;
  209.                 }
  210.                 else {
  211.                     best |= SELJ3;
  212.                 }
  213.             }
  214.         }
  215.         if ((dirs & (SELFX | SELFY | SELFZ)) == SELFY) {
  216.             if ((ct1y > ct2y) && (ct1y > ct3y)) {
  217.                 best |= SELJ1;
  218.             }
  219.             else {
  220.                 if (ct2y > ct3y) {
  221.                     best |= SELJ2;
  222.                 }
  223.                 else {
  224.                     best |= SELJ3;
  225.                 }
  226.             }
  227.         }
  228.         if ((dirs & (SELFX | SELFY | SELFZ)) == SELFZ) {
  229.             if ((ct1z > ct2z) && (ct1z > ct3z)) {
  230.                 best |= SELJ1;
  231.             }
  232.             else {
  233.                 if (ct2z > ct3z) {
  234.                     best |= SELJ2;
  235.                 }
  236.                 else {
  237.                     best |= SELJ3;
  238.                 }
  239.             }
  240.         }
  241.     }
  242.     if ((dirs & (SELMX | SELMY | SELMZ)) == (SELMX | SELMY | SELMZ)) {
  243.         best |= SELJ4 | SELJ5 | SELJ6;
  244.     }
  245.     else {
  246.         if (dirs & SELMX) {
  247.             best |= SELJ4;
  248.         }
  249.         if (dirs & SELMY) {
  250.             best |= SELJ5;
  251.         }
  252.         if (dirs & SELMZ) {
  253.             best |= SELJ6;
  254.         }
  255.     }
  256.     return(best);
  257. }
  258.  
  259. #endif
  260.  
  261.  
  262. #ifdef STAN
  263.  
  264. select_n(dirs, tool) /*::*/
  265. register int dirs;
  266. register TRSF_PTR tool;
  267. {
  268.     register int best = 0;
  269.  
  270.     if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFY | SELFZ)) {
  271.         best |= SELJ1 | SELJ2 | SELJ3;
  272.     }
  273.     else {
  274.         real ct1x, ct1y, ct1z, ct2x, ct2y, ct2z, ct3x, ct3y, ct3z;
  275.         TRSF tot;
  276.  
  277.         if (tool != NULL) {
  278.             Assigntr(&tot, tool);
  279.         }
  280.         else {
  281.             Assigntr(&tot, unitr);
  282.         }
  283.         if (dirs & SELFX) {
  284.             ct1x = tot.n.x * sncs_d.d1x +
  285.                    tot.n.y * sncs_d.d1y +
  286.                    tot.n.z * sncs_d.d1z;
  287.             ct1x = FABS(ct1x);
  288.  
  289.             ct2x = tot.n.x * sncs_d.d2x +
  290.                    tot.n.y * sncs_d.d2y +
  291.                    tot.n.z * sncs_d.d2z;
  292.  
  293.             ct3x = tot.n.x * sncs_d.d3x +
  294.                    tot.n.y * sncs_d.d3y +
  295.                    tot.n.z * sncs_d.d3z;
  296.  
  297.             ct2x = ct3x + ct2x;
  298.             ct2x = FABS(ct2x);
  299.  
  300.             ct3x = FABS(ct3x);
  301.         }
  302.         if (dirs & SELFY) {
  303.             ct1y = tot.o.x * sncs_d.d1x +
  304.                    tot.o.y * sncs_d.d1y +
  305.                    tot.o.z * sncs_d.d1z;
  306.             ct1y = FABS(ct1y);
  307.  
  308.             ct2y = tot.o.x * sncs_d.d2x +
  309.                    tot.o.y * sncs_d.d2y +
  310.                    tot.o.z * sncs_d.d2z;
  311.  
  312.             ct3y = tot.o.x * sncs_d.d3x +
  313.                    tot.o.y * sncs_d.d3y +
  314.                    tot.o.z * sncs_d.d3z;
  315.  
  316.             ct2y = ct2y + ct3y;
  317.             ct2y = FABS(ct2y);
  318.  
  319.             ct3y = FABS(ct3y);
  320.         }
  321.         if (dirs & SELFZ) {
  322.             ct1z = tot.a.x * sncs_d.d1x +
  323.                    tot.a.y * sncs_d.d1y;
  324.             ct1z = FABS(ct1z);
  325.  
  326.             ct2z = tot.a.x * sncs_d.d2x +
  327.                    tot.a.y * sncs_d.d2y;
  328.  
  329.             ct3z = tot.a.x * sncs_d.d3x +
  330.                    tot.a.y * sncs_d.d3y;
  331.  
  332.             ct2z = ct2z + ct3z;
  333.             ct2z = FABS(ct2z);
  334.  
  335.             ct3z = FABS(ct3z);
  336.         }
  337.         if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFY)) {
  338.             if ((ct1x > ct2x) && (ct1x > ct3x)) {
  339.                 best |= SELJ1;
  340.                 if (ct2y > ct3y) {
  341.                     best |= SELJ2;
  342.                 }
  343.                 else {
  344.                     best |= SELJ3;
  345.                 }
  346.             }
  347.             else {
  348.                 if (ct2x > ct3x) {
  349.                     best |= SELJ2;
  350.                     if (ct1y > ct3y) {
  351.                         best |= SELJ1;
  352.                     }
  353.                     else {
  354.                         best |= SELJ3;
  355.                     }
  356.                 }
  357.                 else {
  358.                     best |= SELJ3;
  359.                     if (ct1y > ct2y) {
  360.                         best |= SELJ1;
  361.                     }
  362.                     else {
  363.                         best |= SELJ2;
  364.                     }
  365.                 }
  366.             }
  367.         }
  368.         if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFZ)) {
  369.             if ((ct1x > ct2x) && (ct1x > ct3x)) {
  370.                 best |= SELJ1;
  371.                 if (ct2z > ct3z) {
  372.                     best |= SELJ2;
  373.                 }
  374.                 else {
  375.                     best |= SELJ3;
  376.                 }
  377.             }
  378.             else {
  379.                 if (ct2x > ct3x) {
  380.                     best |= SELJ2;
  381.                     if (ct1z > ct3z) {
  382.                         best |= SELJ1;
  383.                     }
  384.                     else {
  385.                         best |= SELJ3;
  386.                     }
  387.                 }
  388.                 else {
  389.                     best |= SELJ3;
  390.                     if (ct1z > ct2z) {
  391.                         best |= SELJ1;
  392.                     }
  393.                     else {
  394.                         best |= SELJ2;
  395.                     }
  396.                 }
  397.             }
  398.         }
  399.         if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFY | SELFZ)) {
  400.             if ((ct1y > ct2y) && (ct1y > ct3y)) {
  401.                 best |= SELJ1;
  402.                 if (ct2z > ct3z) {
  403.                     best |= SELJ2;
  404.                 }
  405.                 else {
  406.                     best |= SELJ3;
  407.                 }
  408.             }
  409.             else {
  410.                 if (ct2y > ct3y) {
  411.                     best |= SELJ2;
  412.                     if (ct1z > ct3z) {
  413.                         best |= SELJ1;
  414.                     }
  415.                     else {
  416.                         best |= SELJ3;
  417.                     }
  418.                 }
  419.                 else {
  420.                     best |= SELJ3;
  421.                     if (ct1z > ct2z) {
  422.                         best |= SELJ1;
  423.                     }
  424.                     else {
  425.                         best |= SELJ2;
  426.                     }
  427.                 }
  428.             }
  429.         }
  430.         if ((dirs & (SELFX | SELFY | SELFZ)) == SELFX) {
  431.             if ((ct1x > ct2x) && (ct1x > ct3x)) {
  432.                 best |= SELJ1;
  433.             }
  434.             else {
  435.                 if (ct2x > ct3x) {
  436.                     best |= SELJ2;
  437.                 }
  438.                 else {
  439.                     best |= SELJ3;
  440.                 }
  441.             }
  442.         }
  443.         if ((dirs & (SELFX | SELFY | SELFZ)) == SELFY) {
  444.             if ((ct1y > ct2y) && (ct1y > ct3y)) {
  445.                 best |= SELJ1;
  446.             }
  447.             else {
  448.                 if (ct2y > ct3y) {
  449.                     best |= SELJ2;
  450.                 }
  451.                 else {
  452.                     best |= SELJ3;
  453.                 }
  454.             }
  455.         }
  456.         if ((dirs & (SELFX | SELFY | SELFZ)) == SELFZ) {
  457.             if ((ct1z > ct2z) && (ct1z > ct3z)) {
  458.                 best |= SELJ1;
  459.             }
  460.             else {
  461.                 if (ct2z > ct3z) {
  462.                     best |= SELJ2;
  463.                 }
  464.                 else {
  465.                     best |= SELJ3;
  466.                 }
  467.             }
  468.         }
  469.     }
  470.     if ((dirs & (SELMX | SELMY | SELMZ)) == (SELMX | SELMY | SELMZ)) {
  471.         best |= SELJ4 | SELJ5 | SELJ6;
  472.     }
  473.     else {
  474.         if (dirs & SELMX) {
  475.             best |= SELJ4;
  476.         }
  477.         if (dirs & SELMY) {
  478.             best |= SELJ5;
  479.         }
  480.         if (dirs & SELMZ) {
  481.             best |= SELJ6;
  482.         }
  483.     }
  484.     return(best);
  485. }
  486.  
  487. #endif
  488.