.ND
.EQ
delim $$
.EN
.nr H1 7
.NH 1
Structuring Programs
.PP
We shall attempt in this section to show how higher level primitives
can be written in term of RCCL functions.
We shall make use of the macro processing facilities to
define in a few lines some manipulator language statements
often encountered.
A primitive
.B insert
based on a bare bone version of the insertion task explained earlier
is described.
This
.B insert
primitive, newly defined is used in a repetitive task.
Each loop the manipulator moves to a `get' position where
a feeder conveys pegs to be inserted on an assembly.
The holes locations are stored on file and may have been taught in a
previous operation or obtained from a CAD/CAM system.
The loop synchronizes with the feeder's actions via an external
variable :
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "umac.h"
     3  #include "hand.h"
     4
     5  #define AWAYZ(p, l)      {distance("dz", - (l)); move(p);}
     6  #define OVERSHOOTZ(p, l) {distance("dz",   (l)); move(p);}
     7  #define FAST             setvel(300, 300.);
     8  #define SLOW             setvel(50, 50.);
     9  #define CAUTIOUS         setvel(7, 7);
    10
    11  /*
    12   * do one insertion
    13   */
    14
    15  insert(z, grip, peg, hole, depth, ang)
    16  TRSF_PTR z, grip, peg, hole;
    17  real depth, ang;
    18  {
    19          TRSF_PTR bottom, angle, roty;
    20          POS_PTR align, in, touch;
    21
    22          bottom = gentr_trsl("BOTTOM", 0., 0., -depth);
    23          angle = gentr_rot("ANGLE", 0., 0., 0., yunit, ang);
    24          roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
    25
    26          align = makeposition(
    27          "ALIGN", z, t6, grip, peg, EQ, hole, angle, roty, TL, peg);
    28
    29          touch = makeposition(
    30          "TOUCH", z, t6, grip, peg, EQ, hole, angle, roty, TL, peg);
    31
    32          in = makeposition(
    33          "IN", z, t6, grip, peg, EQ, hole, bottom, roty, TL, peg);
    34
.DE
.cs R
.br
.cs R 23
.DS L
    35          setmod('c');
    36          FAST
    37          AWAYZ(touch, 10.)
    38          CAUTIOUS
    39          AWAYZ(touch, 4.)
    40          limit("fz", 25.);
    41          OVERSHOOTZ(touch, 5.)
    42          comply("fz", 15.);
    43                  move(align);
    44          lock("fz");
    45          comply("fx fy", 0., 0.);
    46                  update(hole, in);
    47                  limit("fz", 20.);
    48                  OVERSHOOTZ(in, 10.);
    49          lock("fx fy");
    50
    51          SLOW
    52          AWAYZ(align, 50.)
    53          waitfor(in->end)
    54          OPEN
    55          move(there);
    56          waitfor(completed);
    57          freepos(align);
    58          freepos(in);
    59          freepos(touch);
    60          freetrans(bottom);
    61          freetrans(angle);
    62          freetrans(roty);
    63          return;
    64  }
    65
.DE
.cs R
.br
.cs R 23
.DS L
    66  /*
    67   * monitors feeder
    68   */
    69
    70  #define PARTS   1
    71  #define EMPTY   2
    72
    73  monfeeder()
    74  {
    75          if (feedersensor == PARTS) {
    76                  nextmove = YES;
    77                  CLOSE
    78          }
    79          if (feedersensor == EMPTY) {
    80                  parts = 0;
    81          }
    82  }
    83
    84  /*
    85   * Do insertions
    86   */
    87
    88  int parts = YES;
    89
    90  pumatask()
    91  {
    92          TRSF_PTR z, e, assy, h, feeder, grasp, pegs;
    93          POS_PTR get;
    94
    95          z = gentr();                            /* base frame */
    96          e = gentr();                            /* end effector */
    97          assy = gentr();                         /* assembly */
    98          grasp = gentr();                        /* gasp pos */
    99          feeder = gentr();                       /* feeder */
   100          pegs = gentr();                         /* peg rel. e */
   101          h = newtrans("H", hold);                /* h rel. to assy */
   102
   103          get = makeposition("GET", z, t6, e, EQ, feeder, grasp, TL, e);
   104
   105          while(parts) {
   106                  move(get);
   107                  evalfn(monfeeder);
   108                  setime(200, 10000);
   109                  move(get);
   110                  gettr(h, file);
   111                  insert(z, e, pegs, h, 20., 15.);
   112          }
   113  }
.DE
.cs R
.bp
.PP
Conveyors are expensive, and rugged objects could be thrown from place
to place.
We shall see here how a `throw' primitive (seldom found in regular
robot programming languages) can be easily written.
In order to obtain a maximum acceleration, we shall program a sequence of
motions that only uses the transition part.
This example is only given as an illustration because the
dynamic qualities of the Puma manipulator proved to be not quite sufficient.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "hand.h"
     3  #include "umac.h"
     4
     5  real when;              /* to open the gripper */
     6
     7  #define MAXACC  .015    /* mm/ms2 */
     8
     9  throw(v0)
    10  VECT_PTR v0;
    11  {
    12          int openat();
    13
    14          real Tx = (12. * v0->x) / MAXACC;       /* compute          */
    15          real Ty = (12. * v0->y) / MAXACC;       /* the acceleration */
    16          real Tz = (12. * v0->z) / MAXACC;       /* times            */
    17          int  T = ((FABS(Tx) > (Ty))             /* and pick up      */
    18                          ? ((FABS(Tx) > FABS(Tz))
    19                                  ? Tx : Tz)      /* the longest one  */
    20                          : ((FABS(Ty) > FABS(Tz))
    21                                  ? Ty : Tz));
    22
    23          real dx, dy, dz;
    24
    25          stop(0);
    26          setmod('c');
    27          dx = Tx * v0->x / 2.;
    28          dy = Ty * v0->y / 2.;
    29          dz = Tz * v0->z / 2.;
    30
    31          distance("dx dy dz", -dx, -dy, -dz);
    32          setime(T / 2, T);
    33          move(there);            /* back up      */
    34
    35          when = .90;             /* open the gripper just */
    36          evalfn(openat);         /* before the end        */
    37          distance("dx dy dz", 2. * dx,  2. * dy,  2. * dz);
    38          setime(T / 2, T);
    39          move(there);            /* move as fast as possible */
    40
    41          setime(T / 2, T);       /* come back            */
    42          move(there);
    43          stop(0);
    44          return;
    45  }
    46
    47
    48  openat() /* opens the gripper at a given moment */
    49  {
    50          if (goalpos->scal >= when) {
    51                  OPEN
    52          }
    53  }
.DE
.cs R
.br
.cs R 23
.DS L
    54
    55  pumatask()
    56  {
    57          TRSF_PTR b0, grip;
    58          POS_PTR p0;
    59          VECT vel;
    60          int q;
    61
    62          grip = gentr_trsl("GRIP", 0., 0., 170.);
    63          b0 = gentr_rot("B0", 400., 150., 700., yunit, 45.);
    64
    65          p0 = makeposition("P0", t6, grip, EQ, b0, TL ,grip);
    66
    67          QUERY(q)
    68          CLOSE
    69          setconf("d");   /* elbow down, like the Great Di Maggio */
    70          setime(100, 3000);
    71          move(p0);               /* move above the shoulder */
    72          vel.x = .0;
    73          vel.y = .0;
    74          vel.z = .6;             /* m/s at 45 degrees, see B0 */
    75
    76          throw(&vel);
    77
    78          setmod('j');            /* back to park */
    79          setconf("u");           /* elbow up     */
    80          setime(100, 3000);
    81          move(park);
    82  }
.DE
.cs R
The acceleration times , lines 14 to 21, and the magnitudes, lines 27 to 29,
are derived from the coefficients of
the quartic polynomial
functions used to generate the transitions [2].
The segment times are exactly twice the accelerations times.
.bp
.NH
Limitations
.NH 2
Force Control
.PP
In the case of the Puma manipulator,
the implementation of force control suffers a number a limitations
due to the simplicity of the implemented method.
Force measurements are obtained by monitoring the motor currents.
Coulomb friction terms, at the joint level,
have been experimently measured [8].
When the velocity of a joint is small or null, these terms are
irrelevant and cannot be used to improve the accuracy of the control.
When the arm if to stop on force, this is of little importance since
the joints likely to provide the guarded motion are moving.
Nevertheless, this fact has to be kept in mind.
Gravity loadings have also been experimently measured.
Experiments have shown that although the mass of an object carried by
the manipulator could be measured, the accuracy is not sufficient
and is likely to cause offset errors for the gravity loadings.
The function
.B massis
has been implemented to get around this.
.PP
Force specifications possess an estimated accuracy
of approximately 10 Newton in most of the work space.
This is pretty close to the load capabilities of the manipulator,
therefore extreme prudence is recommanded.
Despite this lack of accuracy, the tasks using force control
explained in this document all run with a good reliability.
.PP
When the manipulator transitions from
.I comply
servo mode to
.I position
servo mode, a glitch often occurs and is as noticeable as the velocity is high
and the load important.
It is usually harmless, and correspond to the position
servo correcting the first setpoint.
.PP
Compliance in a given direction is obtained by selecting the
joints most suitable to provide the desired effects [2].
The joint selection method is simplified.
It does not take into account the translation part the
.I tool
frame.
This means that in
.I comply
servo mode, force specifications will always
match the inner joints (1, 2, 3) and torques specifications the wrist
joints (4, 5, 6).
Although the method is reliable and simple, it suffers
the drawback that no remote center of compliance can be specified.
Time constraints have prevented further work on this points, and
any contributions are welcome.
.NH 2
Machine Errors.
.PP
When the robot task is running in real time, the process
is locked into core memory and the interrupt function
of the system as well as the user's background functions
are run at very high priority in kernel mode.
Any system call (machine trap), will crash the system (beware of the prints).
The same problem occurs for any machine error like a bad memory reference
of a floating point exception in any part of the process.
Some debugging tools are provided as explained later.
.NH 2
Process Size
.PP
When the real time process is run, it is locked into core memory
and the virtual memory system desactivated.
Therefore, the process cannot grow it's allocated region.
Dynamic allocation is performed within a preallocated memory area.
The system calls like `malloc' are replaced by alternative
functions [6].
A set a macros :
.br
.cs R 23
.DS L
#define malloc  malloc_l
#define free    free_l
#define realloc realloc_l
#define calloc  calloc_l
#define cfree   cfree_l

.DE
.cs R
allows the user to safely write :
.br
.cs R 23
.DS L
        p = malloc(20);
.DE
.cs R
.PP
This causes a more annoying problem when it comes to opening files.
Files can be opened only when the real time channel is closed.
However, the user can always code :
.br
.cs R 23
.DS L
        ...
        move(p);
        stop(200);
        waitfor(completed);
        release("opening files");
        fd1 = creat(...
        fd2 = open(...

        startup();
        move(...);
.DE
.cs R
The process is temporally put back in normal mode by the function
.B release
[6], and file `opens' can
take place.
The function
.B startup
will resume real time execution by the depressing the ARM POWER
button when requested by the system.
Failing to follow the procedure will also cause a system crash.
.NH 2
Sample
.PP
The sample period is normally 28 ms.
It can be set to 14 via the function
.B sample
and when not needed, the sample period can be reset to 28 ms.
Changing the sample period can cause a slight glitch.
If the velocity if the manipulator is small, the glitch is negligible.
For example the for loop of the example section 7.3
can be coded :
.br
.cs R 23
.DS L
    32          for (; ; ) {
    33                  setvel(400, 300);
    34                  move(get);
    35                  move(p1);
                        stop(0);
    36                  setvel(100, 100);
    37                  sample(15);
    38                  move(p2);
                        stop(0);
    39                  sample(30);
    40                  printf("more ?"); QUERY(q); if (q == 'n') break;
    41          }
.DE
.cs R
.PP
If the user's background functions take to much time to execute, the
behavior of the real time interface no always easy to predict.
In the best cases, it causes a crash of the superviser program running
in the LSI-11.
The arm power is immediately turned off, and nothing annoying happens.
The superviser is restarted and everything comes back to
normal.
It seems that when the user's functions processing time is slightly too long,
the VAX still accepts interrupts, but stacks them and this quickly
causes a system crash.
Finally, if the interrupt code is very long (an infinite loop, for example),
the system is totally blocked and a manual boot is necessary.
.NH 2
Large Rotations.
.PP
For a reason that has not been yet determined, some motion transitions
involving large rotations do not behave quite correctly.
.NH
The Planner and Play Program
.PP
In order to write and debug the first draft of manipulator
programs, a special library is provided.
This library has exactly the same entry points as the regular
library, but replaces the interrupt code with a loop.
Exactly the same programs can by run and tested.
The synchronization features are simulated so that everything happens
in the same order as in the real time version.
The user is advised to run the programs in this mode
before actual execution.
The resulting sequence of points can be dumped onto file
for execution by the
.I play
program [6].
Trajectories can be also stored and displayed on the terminal
by a special program
called
.I dsp.
.PP
When programs with guarded motions are run in this fashion,
the conditions will never be met, unless
special simulation monitoring functions are written.
When programs include comply statements, the comply mode is
simulated as follow :
the compliant joints are selected according to the geometry of the task
and are artificially frozen as if the resulting forces would keep
them immobile.
The accommodation motions compensation feature being still activated,
it may produce
funny but meaningful trajectories.
Tracking with external information can produce various results
according to the situation at hand.
Nevertheless it is very useful to test ahead manipulator programs.
All  branches must be tested because manipulator control is
essentially programming with side effects.
It is always useful to `play' the resulting trajectories in free
space to get an idea of what is going to happen.
.bp
.NH
Program Options
.PP
Programs can be run with a number of options :

.IP v
This option allows the user to specify the printing of information.
A file called `@@.out' is created in the current directory.
It contains informations about what the system understood of the
calls to
.B makeposition.
A record will be printed for each move request.
For the planning version only, a record will be printed by
the trajectory generator at the time the request is executed, for example
the beginning of the file `@@.out' for the camera guided task
Section 7.1. is :
.br
.cs R 23
.DS L
makeposition, pos       "LOOK"  Z T6 E  = EXPECT
optim, initial equation :       T6 =  -Z EXPECT -E
optim, final equation   :       T6 =  _TEMP1 -E
        "COORD":  "TOOL": -E  "POS": _TEMP1

makeposition, pos       "GET"   T6 E  = COORD CAM O
optim, initial equation :       T6 =  COORD(h) CAM O(h) -E
optim, final equation   :       T6 =  COORD(h) CAM O(h) -E
        "COORD": COORD(h) CAM  "TOOL": -E  "POS": O(h)

makeposition, pos       "PUT"   Z T6 E  = DROP
optim, initial equation :       T6 =  -Z DROP -E
optim, final equation   :       T6 =  _TEMP2 -E
        "COORD":  "TOOL": -E  "POS": _TEMP2

.DE
.cs R
.br
.cs R 23
.DS L
request LOOK mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000

PARK -1 28 j 84 84 280 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   00 0 0 0 0 0 0
LOOK -1 336 j 56 56 2660 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   00 0 0 0 0 0 0
.DE
.cs R
.br
.cs R 23
.DS L
request GET mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000
dist dz : -30

request GET mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000

request STOP mode j acct 0  sgt 28  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000

request GET mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000
dist dz : -30

GET -1 3024 j 56 56 1568 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   04 0 0 -30 0 0 0
GET -1 4592 j 56 56 280 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   00 0 0 0 0 0 0
GET -1 4872 j 56 56 140 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   00 0 0 0 0 0 0
.DE
.cs R
.br
.cs R 23
.DS L
request PUT mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000

GET -1 5012 j 56 56 280 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   04 0 0 -30 0 0 0
PUT -1 5292 j 56 56 2492 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   00 0 0 0 0 0 0
PUT -1 7784 j 56 56 112 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   00 0 0 0 0 0 0
request LOOK mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000

Etc ...

.DE
.cs R
The equations are printed, then their canonized form before and
after optimization.
Transforms are marked according to their type : varb (v), hold (h),
functional (s).
The optimization premultiplications generate the `_TEMPx' names.
For each request all the parameters are printed, for example :
.br
.cs R 23
.DS L
request LOOK mode j acct 56  sgt 0  velt 200 velr 100
conf    upd :  smpl 0 mass 0.000000
.DE
.cs R
means : position `LOOK', mode `joint', acceleration time 56 ms,
segment time is 0 that is : will be computed at execution time,
current velocities are 200 mm/s and 100 deg/s, no configuration change
required, no transform to update, no sample time change, current mass of
object is 0. kg.
.IP
The trajectory generator prints in a compact format the specification
at the beginning of each motion (planning version only) :
.br
.cs R 23
.DS L
GET -1 5012 j 56 56 280 28
        force 00 0 0 0 0 0 0
        cply  00 0 0 0 0 0 0
        dst   00 0 0 0 0 0 0
        exd   04 0 0 -30 0 0 0
.DE
.cs R
means : previous motion terminated `OK' (-1), time is 5012 ms,
mode is `joint', accelerations times first transition is 56, second 56,
segment time is 280, time increment is 28.
No force limit, no comply, no differential motion stop, distance is
-30 mm in Z direction.
For the records `force', `cpy', `dst', and `exd', the first number is
an octal code (00 for no specification, translation or forces :
01 for X, 02 for Y, 04 for Z, rotations or torques : 10 for X, 20 for Y,
40 for Z, and the combinations : 01, 03, 05, 06, etc ...);
.IP
If the the option `-vv' as very verbose is given, the values
of the transforms created by the `gentr...' style function is also
printed.
.IP
This option corresponds to the global flag
.B prints_out.
This flag can be turned on or off the text of the programs
themselves :
.br
.cs R 23
.DS L
        ...
        prints_out = YES;
        p0 = makeposition(...);
        p1 = makeposition(...);

        move(p0);
        move(p1);
        prints_out = NO;
.DE
.cs R
The information is printed to the
.B fpi
file pointer :
.br
.cs R 23
.DS L
        FILE *fpi;
.DE
.cs R
This file pointer is initialized to the `stderr' file pointer.
When the flag
.B prints_out
is set to a non zero value, the
.B makeposition
and
.B move
messages go to the terminal.
When the option `-v' is specified, the file `@@.out' is created and
.B fpi
points to it, and the messages are stored on the file.
One can use this feature for any purposes, for example :
.br
.cs R 23
.DS L
pumatask()
{
        TRSF_PTR ...
        POS_PTR ...
        ...

        prints_out = NO;
        p = makeposition(...);
        ...

        move(p);
        ...
        fprintf(fpi, "bla bla");
        ...
}
.DE
.cs R
If the task is run without option `-v', "bla bla" goes on `stderr' file,
if the task is run with option `-v', "bla bla" goes into `@@.out'.

.IP g
This is the `graphic' option (planing version only).
The setpoints are stored in the
files `../g/f1.out', `../g/f2.out', one for each joint.
When displayed with the program
.I dsp
a character `J' stands for joint mode straight part, `T' for joint mode
transition, `E' for first point of joint mode transition, `C' for
a Cartesian mode straight part, `H' for Cartesian transition, and `V'
for first point of Cartesian transition.
In order to use this option, the user is required to have a `graphic'
directory `../g' at the same level in the file tree
hierarchy as the current directory.
This will avoid having the current directory constantly clustered with
junk files.

.IP d
This is the `data' option (planning version only),
when specified, the system creates
the file `@.out' in the current directory that will contain one line
per setpoint according to the following format :
.br
.cs R 23
.DS L
     POS M  time tseg j1  j2  j3  j4  j5  j6 sel
.DE
.cs R
Where `POS' is the name of the goal position, `M' is the mode
(J, T, E, C ,V ,H)
as described above, `j1'...`j6' are the joint angles in range coordinate [6],
and `sel' an octal value showing which joint are complying in comply mode
(0 no joint, 01 for joint 1 , 02 for joint 2, 04 for joint 3, 10 for joint 4,
20 for joint 5, 40 for joint 6, 3 for joint 1 and 2, etc. ).

.IP a
This option when set, causes the joint angles to be output in solution
coordinates [6].
It serves for option `d' and `g'.

.IP k
This option when set, causes the values of $T6$ to be printed in lieu
of the joint angles.
For the option `g' twelve files (f1.out ... f12.out) are created,
the values of the vectors `p', `n', `o', and `a';
For the option `d' the format becomes :
.br
.cs R 23
.DS L
     POS M  time tseg px  py  pz  nx  ny  nz ox oy oz ax ay az
.DE
.cs R
It serves for option `d' and `g'.

.IP e
This option causes the file `@@@.out' to be created in the
current directory (planing version only).
The file contains the sequence of encoder values suitable to be
used by the
.I play
program [6].

.IP "Dname "
This option specifies the file `name' as a data base of transforms.
Can be used in association with the teach mode (see below).
.IP
This option corresponds to the global file descriptor
.B fddb
initialized to `-1'.
When the option `-Dname' is specified,
.B fddb
describes the file `name'.
If the file `name' does not exit the user is prompt as :
.br
.cs R 23
.DS L
name does'nt exit, create ? (y/n)
.DE
.cs R
Answer as appropriate.

.IP b
This option turns off the force control features (brute option).
In the case of the planning version, no simulated joint accommodation
will occur.
In the case of the real time version, it allows us to test the manipulator
programs free of obstacles.
.IP
This option corresponds to the global flag
.B force_ctl
which is turn off by the `-b' option.
The flag can be turned on or off in the text of the programs.
