Programmer 7500/MAX_PROGRAMMERS.iso/INFO/C/ROBOT01.ZIP/RCCL015

Text File | 1987-03-02 | 55.0 KB | 1,739 lines

.ND
.EQ
delim $$
.EN
.nr H1 5
.NH 1
Sensor Integration
.PP
By using sensors, the user has the ability to write
programs that may depend on information acquired at run time.
The behavior of the manipulator can be influenced
by modifying the locations it is moving to or by interrupting a motion.
If the location can be determined ahead of time, we shall call that
presetting the world model.
A special case is the transforms initializations.
If the locations can be determined synchronously and permit us
to influence the manipulator's path, we shall call that tracking.
If the locations can be determined by stopping the manipulator on
condition, we shall call that guarded motions.
If the final position of the manipulator is to be retained for
the determinations of locations, we shall call that updating the world
model.
.NH 2
Presetting the World Model.
.PP
In the section `Synchronization' we have already met such a situation.
The example of a program interacting with a user was given :
.br
.cs R 23
.DS L
        for (; ; ) {
                printf("enter Z increment ");
                scanf("%f", &iz);
                b->p.z += iz;
                move(p);
        }
.DE
.cs R
The
.I hold
transform feature allowed us the specify different locations ahead
at time and no synchronization is specified.
.PP
Let us consider the integration of a computer vision system.
We assume that a camera has been attached to the link 4 of
the PUMA manipulator.
The computer vision is described in terms of a functions `snapshot'
which is supposed to take a picture of the scene and store it in memory, and
of a function `getobj' able to extract the position and orientation
of an object in the camera coordinate frame.
The operation of taking the picture is expected to be short but the task
is programmed in such a way that the processing time of `getobj' does not
require to stop the arm.
The strategy consist of moving the manipulator toward a position
where we expect an object to be captured in the field of the camera.
We synchronize the program such as the picture is taken at a given
point of the trajectory.
We also record at this instant the position of the manipulator
given by $T6$ and we reconstruct the camera coordinate frame from
the transform $U5$ internally maintained by the system.
We could also compute the values of the transform $T4$ since we can know
at any moment the joint angles values (see include files).
Thus we have all the information necessary to perform an
approach motion where the object has been found, grasp it, and move it
to some other place.
.PP
A bare bone version of the task is described
in terms of three position equations : the position where to expect
an object to be seen by the camera, the position where to grasp the object,
and the position where to put it :
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "hand.h"
     3  #include "which.h"
     4  #include "kine.h"
     5
     6  pumatask()
     7  {
     8          TRSF_PTR  z, e, cam, o, coord, t6r, u5i, expect, drop;
     9          POS_PTR look, get, put;
    10
    11          z = gentr_trsl("Z",  0.,  0., 864.);
    12          e = gentr_trsl("E" , 0. , 0. , 170.);
    13          cam = gentr_rot("CAM", 0., 0., 50., xunit, 90.);
    14          expect = gentr_rot("EXPECT", 500. , 100., 600., yunit, 180.);
    15          drop = gentr_rot("DROP", 400. , -100., 500., yunit, 180.);
    16          o = newtrans("O", hold);
    17          coord = newtrans("COORD", hold);
    18
    19          u5i = newtrans("U5I", varb);
    20          t6r = newtrans("T6R", varb);
    21
    22          look = makeposition("LOOK", z, t6, e, EQ, expect, TL, e);
    23          get = makeposition("GET", t6, e, EQ, coord, cam, o, TL, e);
    24          put = makeposition("PUT" , z, t6, e, EQ, drop, TL, e);
    25
.DE
.cs R
.br
.cs R 23
.DS L
    26
    27          setvel(200, 100);
    28          for (; ; ) {
    29                  move(look);
    30                  waitas(goalpos == look && look->scal > .8);
    31                  snapshot();
    32                  Assigntr(t6r, t6);
    33                  Invert(u5i, &sncs_d.u5);
    34                  Trmult(coord, t6r, u5i);
    35                  if (!getobj(o)) {
    36                          break;
    37                  }
    38                  get->end = 0;
    39                  distance("dz", -30.);
    40                  move(get);
    41                  move(get);
    42                  stop(50);
    43                  distance("dz", -30.);
    44                  move(get);
    45
    46                  waitfor(get->end);
    47                  waitfor(get->end);
    48                  CLOSE;
    49                  printf("closing\\\\n");
    50                  move(put);
    51                  waitfor(put->end);
    52                  OPEN;
    53                  printf("opening\\\\n");
    54          }
    55          move(park);
    56  }
    57
    58  snapshot()
    59  {
    60          printf("snap\\\\n");
    61  }
    62
    63  getobj(t)
    64  TRSF_PTR t;
    65  {
    66          static int number = 5;
    67
    68          Trsl(t, 0., 0., 200. + number * 30.);
    69          Rot(t, yunit, 10. * number);
    70          return(number--);
    71  }
.DE
.cs R
.PP
Line 1 includes RCCL declarations.
Line 2 includes the file
.B hand.h
that contain the two macros `OPEN' and `CLOSE' to actuate the pneumatic
gripper.
Line 4 includes the file
.B kine.h
that contains manipulator dependent informations about the kinematics.
This file contains the structure declarations and external
declarations of variables internally used by RCCL.
Since this file depends on the manipulator type it must be preceded
with the definition of the particular manipulator
(`PUMA' for the Puma 600, `STAN' for the stanford arm).
The file
.B which.h
included line 3 contains the line : ``#define PUMA'' that describe
the current implementation.
We are primarily interested in the variable called
.B snsc_d
declared in
.B kine.h
as :
.br
.cs R 23
.DS L
typedef struct sincos {
        real c1, s1, c2, s2, c23, s23, c3, s3, c4, s4, c5, s5, c6, s6;
        real d1x, d1y, d1z, r1x, r1z, d2x, d2y, d2z, d3x, d3y, d3z;
        real h;
        TRSF u5;
} SNCS, *SNCS_PTR;

extern  SNCS  sncs_d;
.DE
.cs R
The elements of
.B sncs_d
are kinematic parameters updated at sample time that the user may
use.
For the Puma manipulator,
the first line is the list of sines and cosines values of each joint
angles.
The second and third line exhibit the coefficients
of the Jacobian matrix that contain multiplications computed
in link 4 [9].
The fourth line of type `TRSF' is the transform $U5$ and we shall
make use of it.
.PP
Back to the program, after the pointer declaration we find, lines 11 through 20,
the allocation of transforms.
For simplicity, we will name them the same way as
the frames that they describe.
.IP "Z : "
a reference frame at the base of the manipulator.

.IP "E : "
the end effector frame.

.IP "CAM : "
the camera frame described with respect to link 4.

.IP "EXPECT : "
the position where we expect to find an object in the camera field
with respect to the Z frame.

.IP "DROP : "
the position from where we would like the manipulator to drop the
object.

.IP "O : "
The position of the object in camera coordinates that we declare as
.I hold
since it will be changed at during the task execution.

.IP "COORD : "
The position of the camera in base coordinates that is changing as the
manipulator moves.

.IP "U5I and T6R : "
auxiliary transforms that are used to hold the inverse of U5 and a copy
of T6 at the moment of taking the picture.
They will be used to compute the transform COORD, but are not used
in a position equations.
.PP
The three calls to makeposition, lines 22, 23, and 24 define to following
transform graphs :
.br
.cs R 23
.DS L
LOOK :

           T6     E
         -----> ----->
        |             |
        |             |
         <----- ----->
           Z    EXPECT
.DE
.cs R
.PP
This first graph is quite ordinary.
.br
.cs R 23
.DS L
GET :

           T6     E
         -----> -----> <-----
        |                    |
        |                    |
         <----- -----> ----->
          COORD  CAM     O
.DE
.cs R
This graph describes the final position entirely in manipulator coordinates.
The transforms $COORD$ and $O$ are measured at the moment of taking
the picture and their values memorized in the motion queue.
It is mapped on the equivalent graph :
.br
.cs R 23
.DS L
GET :
                 CAM     O
                -----> ----->
               |             |
          T4   |   U5    E   |
         ----->|-----> ----->|
        |             |      |
        |    T6       |      |
        |------------>       |
        |                    |
         <----- -----> ----->
          COORD  CAM     O
.DE
.cs R
from which we can derive :
.EQ
~COORD~=~T6~U5 sup -1
.EN
.br
.cs R 23
.DS L
PUT :

        T6         E
         -----> ----->
        |             |
        |             |
         <----- ----->
           Z    DROP
.DE
.cs R
.PP
Again, an ordinary graph.
.PP
Statement, line 27, sets the velocity and one can notice that
the motion type is left in
.I joint
mode, since we are more interested in the end of path segment
position than with the trajectories.
The body of the program is essentially a loop that will exit when the
function `getobj' returns zero value.
The function `getobj' is simulated and returns five different successive
values for the $O$ transform.
.PP
At the beginning of the loop, line 29, a motion is requested toward
the `LOOK' position.
The program is then synchronized such that `snapshot' is called at
80 per cent of the trajectory.
Note that a `waitas(look->scal > .8)' statement may not be sufficient
since after execution of the first loop the value of `look->scal'
is left at 1. as a side effect of the previous loop,
and no synchronization would be achieved.
Values of $T6$ and $U5$ are copied on the fly, and the $COORD$
transform value computed, lines 32 to 34.
The function `getobj' has the remaining trajectory time
to obtain of value for $O$.
An `approach - grasp - depart' sequence is then performed before
the next loop, lines 39 to 44.
.PP
This program is only a sketch and many improvements
are possible.
For example, we may like to interrupt the motion and proceed to
the grasp sequence as soon as a value for $O$ is obtained.
We shall see in the next section how to achieve this result.
Another variation would be to introduce a conveyor carrying objects
in the vision field of the camera.
The only required modifications would be the position equation for `GET' :
.br
.cs R 23
.DS L
        get = makeposition("GET", z, t6, e, EQ, conv, coord, cam, o, TL, e);
.DE
.cs R
This is because the object position information is entirely contained
in the transforms $T4$, $CAM$, and $O$, and the position `GET'
would be tracking the conveyor.
.NH 2
Guarded Motions
.PP
This section explains how to interrup a motion
on condition.
Interrupting a motion can allow us to stop or to start the arm,
to suspend or resume a motion toward a position.
This can be achieved in all cases by setting the flag called
.B nextmove
at a non zero value.
This causes the motion currently being performed to be interrupted
and the next in the queue to begin.
The value to which the flag
.B nextmove
has been set is returned in the `code' field of the structure `POS',
described earlier.
When using this feature, the user must be careful not to conflict
with values that have a predetermined meaning for RCCL :
.br
.cs R 23
.DS L
#define OK      -1
#define LIMIT   -2
#define ONF     -3
#define OND     -4
.DE
.cs R
For example, one could use only positive values.
.PP
There are basically two ways of using the
flag
.B nextmove.
It can be set either from a background function,
or from the user's foreground process.
Let us illustrate this by an example using a simple sensor which
is a small linear potentiometer.
The distance of which the shaft is inside the body of the potentiometer
can be measured through analog channels.
The robot controller
performs analog to digital conversions
when specified via the function
.B adcopen
and the values can be read from a global array updated at sample intervals [6].
We will make use of this information to program guarded motions.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "hand.h"
     3  #include "rtc.h"
     4  #include "umac.h"
     5
     6  extern struct how how;
     7
     8  static int sensor;
     9
    10  #define TOUCHED 10
    11
    12  pumatask()
    13  {
    14          int touchfn();
    15          TRSF_PTR z, b1, b2, fing, getit, flip;
    16          POS_PTR  get, p1, p2;
    17          int q;
    18
    19          z = gentr_trsl("Z",  0.,  0., 864.);
    20          b1 = gentr_trsl("B1", 600. ,-200., 400.);
    21          b2 = gentr_trsl("B2", 600. ,-100., 400.);
    22          fing = gentr_rot("FING", 0., 0., 200., zunit, -90.);
    23          getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);
    24          flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);
    25
    26          p1 = makeposition("P1" , z, t6, fing, EQ, b1, flip,TL , fing);
    27          p2 = makeposition("P2" , z, t6, fing, EQ, b2, flip,TL , fing);
    28          get = makeposition("GET", z, t6, EQ, getit, TL, t6);
    29
    30          setvel(300, 100);
    31          move(get);
    32          waitfor(completed);
    33          OPEN;
    34          printf("put the sensor in the jaws ");
    35          QUERY(q);
    36          CLOSE;
    37          printf("go ahead ");
    38          QUERY(q);
    39          if (q == 'n') {
    40                  move(park);
    41                  return;
    42          }
.DE
.cs R
.br
.cs R 23
.DS L
    43          sensor = adcopen(7);
    44          setvel(100, 100);
    45          for (; ; ) {
    46                  p1->end = p2->end = 0;
    47                  move(p1);
    48                  evalfn(touchfn);
    49                  setime(0, 0);
    50                  distance("dz", 100.);
    51                  move(p1);
    52                  move(p1);
    53
    54                  move(p2);
    55                  evalfn(touchfn);
    56                  setime(0, 0);
    57                  distance("dz", 100.);
    58                  move(p2);
    59                  move(p2);
    60
    61                  waitfor(p1->end)
    62                  printf("guarded motion 1 starts\\\\n");
    63                  waitfor(p1->end)
    64                  if (p1->code == TOUCHED)
    65                          printf("touched\\\\n");
    66                  else
    67                          printf("not touched\\\\n");
    68
    69                  waitfor(p2->end)
    70                  printf("guarded motion 2 starts\\\\n");
    71                  waitfor(p2->end)
    72                  if (p2->code == TOUCHED)
    73                          printf("touched\\\\n");
    74                  else
    75                          printf("not touched\\\\n");
    76
    77                  printf("more ? ");
    78                  QUERY(q); if (q == 'n') break;
    79          }
    80          setvel(300, 100);
    81          move(park);
    82          waitfor(completed);
    83          OPEN;
    84  }
    85
    86
    87  touchfn()
    88  {
    89          if (how.adcr[sensor] > 1) {
    90                  nextmove = TOUCHED;
    91          }
    92  }
.DE
.cs R
.PP
We are now familiar with lines 1 and 2.
Line 3 includes the real time interface declarations [6], in order
to gain access to the analog conversions.
Line 4 includes the file
.B umac.h
that contains a set of useful macros (see include files),
among them we shall use
the macro `QUERY' that causes the prompt `` (y/n) '' to be printed on the termin
and will return when the user has typed a `y' or a `n'.
The typed character is then returned in the macro's argument.
Line 6 declares the type of the array in which we get the analog readings.
Line 8 allocates an integer that will be the index in the array `adcr'
of the readings of the opened analog channel.
Line 10 defines the return code of the guarded motion.
Let us skip the transforms and position initializations that
do not show anything special.
With a combination of queries we ask the operator to place
the sensor in the gripper's jaws and to command the gripper to close.
We leave to the operator the option of canceling the task on line 39
if something goes wrong.
Line 43 allocates analog channel number 7 to the sensor.
In the body of the loop, lines 45 to 78, the manipulator performs two
guarded motions : moves to a position next to an expected obstacle,
moves along the Z direction in the
.I tool
frame, while evaluating at sample intervals the monitoring function `touchfn'.
The call to
.B setime
specifies a zero transition time at the end of the motion in order to
obtain a fast reaction time.
The null transition time can be specified here as we have made sure
that the velocities are small.
We also make sure that the motion queue contains a position
such as the arm will back up when the obstacle in encountered.
This is the purpose of the move statements lines 52 and 59.
.PP
Using the
.B waitfor
macro, the program can print information
at the terminal as the task proceeds.
In particular, it is possible to decided if the guarded motion
did encounter an obstacle.
The value `TOUCHED' is returned in the `code' part of the positions if
the monitoring function caused a motion interruption, otherwise
the value `OK' is returned.
The monitoring function, lines 86 to 91, checks the analog conversion
reading and sets the flag
.B nextmove
when appropriate.
.PP
Some other combinations are possible, as shown by the following
code patterns :
.br
.cs R 23
.DS L
        move(p);
        evalfn(monitorfn);
        setime(100, 10000);
        move(p);
        waitfor(completed)
        if (p->code != EXPECTED) {
                printf("timeout after 10 seconds\\\\n");
                error();
        }
        move(p1);
.DE
.cs R
causes the arm to stop at the position `p' while evaluating a monitor
function, and the motion to resume on condition.
It is not possible to use a
.B stop
statement here, since
.B stop
keeps all the attributes of the previous motion and we need to specify
a new
.B move
request.
The sequence of code :
.br
.cs R 23
.DS L
        evalfn(monitorfn);
        move(p);
        stop(1000);
        move(p);
.DE
.cs R
does not causes a motion to be interrupted for one second, unless
the position `p' has been reached when the
.B stop
request is executed because it is equivalent to a new motion to the last
position.
Similarly, one must be careful that the
.B stop
statement does not necessarily mean that the manipulator will
stop in absolute coordinates if the position equation for `p' contains
moving coordinate frames.
When an absolute stop is needed or when a motion has to stop
and the manipulator has to remain exactly at the position where
it stopped, RCCL provides a built-in position equation of the
following form :
.EQ
~T6~=~HERE
.EN
where $HERE$ is a transform internally maintained by the system
to be always equal to $T6$ at the end of any path segment.
At startup time, the system issues the following call :
.br
.cs R 23
.DS L
        there = makeposition("THERE", t6, EQ, here, TL, t6);
.DE
.cs R
to implement this feature, where
.B here
is of type TRSF_PTR and
.B there
of type POS_PTR.
The following code pattern shows how we can use the fact that
the flag
.B nextmove
can be set from the user's process to implement a stop on terminal input :
.br
.cs R 23
.DS L
        move(p);
        move(there);
        printf("hit return to interrupt motion ");
        getchar();
        nextmove = YES;
.DE
.cs R
When `return' is hit, the system interrupts the motion toward `p',
and starts a transition to the position `there' that causes the arm to over
shoot by a magnitude as great as the velocity was high.
When the velocity is small and a sharp stop is needed we can write :
.br
.cs R 23
.DS L
        move(p);
        setime(0, 0);
        move(there);
        printf("hit return to interrupt motion ");
        getchar();
        nextmove = YES;
.DE
.cs R
In the same way monitoring can achieved with :
.br
.cs R 23
.DS L
        move(p);
        waitas(goalpos == p)
        p->end = 1;                     /* preset event */
        while(p->end) {
                if (condition) {
                        nextmove = YES;
                }
                suspendfg();
        }
.DE
.cs R
This way of coding can be useful in the cases when
it is not possible to place the condition calculations in the
background function.
.PP
RCCL internally monitors if the joint physical limits are
going to be reached (within a few degrees for each joint).
If such an error condition occurs, the system automatically
issues a move to the `there' position, that causes an immediate stop
next to the limit position and returns the code `LIMIT'
for the motion that caused the error condition.
A new motion is then taken out of queue and the error condition
is reset.
If the new motion persists in causing a joint limit error, the whole
task will abort.
If motions are likely to cause such joint limit errors,
the returned codes should be checked and the appropriate action
undertaken.
.NH 2
Tracking
.PP
Tracking is obtained by synchronously updating functionally defined
transforms from sensor readings.
All the examples given in the section ``Functionally defined motions''
would become examples for tracking motions if we would replace the
parameter `time' by some sensor readings reflecting the position
of the moving coordinate frames.
We shall however explain yet another example using the simple
potentiometer based position sensor introduced in the previous section.
The sensor, placed in the
.I tool
frame, allows the manipulator to track an arbitrary surface intersecting
the Z axis of the tool frame.
The tracking function is written in such a way that it causes the
motion velocity to be proportional to the distance the shaft of the linear
potentiometer
is protruding out of the sensor.
A velocity control of the manipulator
end effector is implemented such as that the shaft is partially inside the
body of the sensor, the velocity along the Z axis
of the
.I tool
frame is controlled to be zero.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "rtc.h"
     3  #include "umac.h"
     4
     5  extern struct how how;
     6
     7  int sensor;
     8
     9  pumatask()
    10  {
    11          TRSF_PTR z, b1, b2, fing, fins, over;
    12          POS_PTR  p1, p2, get;
    13          int fingfn();
    14          int q;
    15
    16          fing = newtrans("FING",fingfn);
    17          Rot(fing, zunit, -90.);
    18          fins = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
    19          z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
    20          b1 = gentr_rot("B1", 600. ,-300., 450., yunit, 180.);
    21          b2 = gentr_rot("B2", 600. , 300., 450., yunit, 180.);
    22          over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
    23
    24          p1 = makeposition("P1" , z, t6, fins, EQ, b1, TL, fins);
    25          p2 = makeposition("P2" , z, t6, fing, EQ, b2, TL, fing);
    26          get = makeposition("GET", z, t6, EQ, over, TL, t6);
    27
    28
    29          sensor = adcopen(7);
    30
    31          setmod('c');
    32          for (; ; ) {
    33                  setvel(400, 300);
    34                  move(get);
    35                  move(p1);
    36                  setvel(100, 100);
    37                  sample(15);
    38                  move(p2);
    39                  sample(30);
    40                  printf("more ?"); QUERY(q); if (q == 'n') break;
    41          }
    42          setvel(400, 300);
    43          setmod('j');
    44          move(park);
    45  }
    46
    47
    48
    49  fingfn(t)
    50  TRSF_PTR t;
    51  {
    52          t->p.z += (how.adcr[sensor] * .01 - 3.) / 3.;
    53  }
.DE
.cs R
.PP
This example uses three positions equations.
The position P1 from where the tracking motion begins, uses
a $TOOL$ transform FINS set as a translation and a rotation
such as to present the sensor with a proper angle.
The position P2 is the end of the tracking motion, the
$TOOL$ transform FING is initialized to be equal to FINS.
However, as the motion progresses, its $p sub z$ element will
be changed by the function `fingfn' in order modify
the trajectory in the desired direction.
The function `fingfn', lines 49 to 53, implements the control law whose
parameters have been experimently determined, in terms of a gain
and an offset.
The sample rate is set at a higher value during the tracking
motion in order to obtain a faster response.
.PP
An interresting variation of this program would be
to record the value of $T6$ as the tracking proceeds.
Since it is not possible to perform any input-output
from a background function, a buffer alternating technique
would need to be implemented : while the background function fills
one buffer, the user's foreground process could dump another one on file.
It would then become possible to replay very long motion sequences,
as they have been recorded or in such a way that the
.I tool
frame would have a fixed angle with respect to the tracking
trajectory as required in welding applications (Section ``Functionally
defined motions'', example 4).
.PP
When the sensory input is too slow or when computations are too lengthy
to be performed in a background function (10 ms cpu time every 28 ms
would really tie the machine down), a pseudo tracking can be obtained
by using an asynchronous loop in the user's process updating a
.I varb
type transform.
For example :
.br
.cs R 23
.DS L
        TRSF_PTR z, e, b, r;
        POS_PTR p0, p1, ... , pt;
        TRSF changes;

        z = ...
        e = ...
        b = ...
        r = ...
        upd = newtrans("UPD", varb);

        p0 = makeposition(...);
        p1 = makeposition(...);
        pt = makeposition("P", z, t6, e, EQ, b, upd, r, TL, e);

        ...

        move(p0);
        move(p1);
        move(pt);

        waitas(goalpos == pt)
        while(goalpos == pt) {
                getsensor(&changes);
                Trmultinp(upd, &changes);
                suspendfg();
        }
        ....
.DE
.cs R
.PP
The function `getsensor' returns alterations to be performed on the
transform $UPD$, that are accumulated by successive multiplications.
The function
.B suspendfg
is used to allow the machine to ``breath''.
The changes should not cause more than a few millimeters or degrees position
steps at the end effector.
.PP
Another way to obtain this type of tracking is to use very short motions and
to synchronize them with the sensory input.
We can take advantage of the fact that no time is spend for parsing
the
.B move
instructions and the motions can be very short.
We will preferably
use the
.I joint
mode because it requires far less computations and the path approximation
remains good if the path segments are small.
As opposed to the previous method, we need to use a
.I hold
transform to keep track of all the successive values.
The
.I hold
transforms have also the property of being retained by the system
from one path to another for the transition calculations.
The code pattern may look like :
.br
.cs R 23
.DS L
        ...
        upd = newtrans("UPD", hold);
        ...

        move(pt);
        waitfor(completed);
        setmod('j');
        rtime = 0;
        while (rtime < tracktime) (
                getsensor(&changes);
                Trmultinp(upd, &changes);
                move(pt);
                waitas(nbrequest < 2);
        }
.DE
.cs R
At the end of the loop, the
.B waitas
primitive allows the loop to proceed an perform another sensorial input
while the arm is still moving.
A look ahead is thus implemented although
it may lead to an unstable control.
If the loop is strongly synchronized by a `waitas(nbrequest < 1)',
the manipulator will stop at each loop before proceeding.
Since the successive motions are expected to be small, we
can afford to suppress of the transitions by a call to `setime(0, 0)'.
In the above examples, we have implemented the tracking in terms of
.B changes
with respect to the current position.
In other words, we assume that the sensor is linked with the
manipulator and that the changes to be performed represent the tracking
error.
.PP
Guarded motions can also be implemented using a similar technique :
.br
.cs R 23
.DS L
        setmod('j');
        while (getsensor() == goahead) {
                Trmulinp(upd, &step);
                setime(0, 0);
                move(pt);
                waitfor(pt->end);
        }
.DE
.cs R
.PP
According to the situation, different path segment times
or velocities
as well as strategies can to be experimented.
.NH 2
Updating the World Model
.PP
For all motions terminated on a condition, keeping track of
the position where the manipulator stopped is another way
of acquiring information from the environment.
Instead of merely recording the position of the manipulator,
the
.B update
primitive allows the user to record positions in a structured manner.
The
.B update
function takes two arguments, a transform pointer, and a position
equation pointer :
.br
.cs R 23
.DS L
        update(trans, pos)
        TRSF_PTR trans;
        POS_PTR pos;
.DE
.cs R
The
.B update
function causes the position equation `pos'
to be solved for the transform `trans',
using the value of $T6$ at the end of the corresponding path segment.
Of course, the transform must belong to the position equation.
The transform must also be of type
.I varb.
The second argument, a position equation pointer, is not necessarily
the same argument as the one of the corresponding motion statement.
For example, we can update a transform on user request :
.br
.cs R 23
.DS L
        a = gentr...
        e = gentr...
        y = gentr...
        z = gentr...

        x = newtrans("X", varb);

        p1 = makeposition("P1", z, t6, e, EQ, a, TL, e);
        p2 = makeposition("P2", z, t6, e, EQ, x, y, TL, e);

        ...

        update(x, p2);
        move(p1);
        move(p2);
        printf("hit return to interrupt motion ");
        getchar();
        nextmove = YES;
.DE
.cs R
An update request is associated with position P1.
When the user hits `return', the motion toward P1 is interrupted and
the transform X is solved as :
.EQ
X~=~Z~T6~E~Y sup -1
.EN
and the position P2 corresponds exactly to the position the manipulator
was and a stop is obtained.
Subsequent motions to this position are therefore possible.
The transform X can also be used in other position equations.
One must notice that all the positions containing X are consequently changed.
This leads to
numerous applications of
.B update.
.bp
.NH
Force Control
.PP
In assembly tasks, objects are required to be brought into contact,
and motions have to be stopped when the collision occurs.
Once objects are in contact the task is said to be constrained
because arbitrary motions are no longer possible in every direction.
The notion of guarded motion has been introduced earlier and
force guarded motions are quite similar.
The force monitoring function is built into the system
considering that it is somewhat dependent on the installation and
that force specifications are really an integral part of a motion
description.
Force specifications are transmitted to the background process via
the
.B limit
primitive.
When the task is constrained, the arm is requested to exert
forces on objects and is no longer position servoed for
each of the six degrees of freedom.
Depending on the geometry of the task, one or several directions
are selected to provide for compliance.
The arm is then said to enter a
.I comply
mode which is specified by the
.B comply
primitive.
When the contact between objects ceases,
or when constraints disappear,
the arm has to gain back position servoed directions.
This is achieved by the
.B lock
primitive.
The cessation of contact can be detected by
differential motions of the arm when the constraints disappear.
The primitive
.B limit
is also used for this purpose.
.NH 2
Stop, Go on Force, on Displacement
.PP
As we have seen before, stop and go are not essentially different, they
both correspond to the interruption of a motion.
When a limit condition is specified, a monitor function is internally
activated for the subsequent motion.
The form of
.B limit
is :
.br
.cs R 23
.DS L
        limit(dirs, value [, value] ...)
        char *dirs;
        double value;
.DE
.cs R
The limit directions specifications are expressed in the string
first argument with a combination of the following, separated by one
or several spaces :
.br
.cs R 23
.DS L
        fx fy fz : force         along X Y Z
        tx ty tz : torque        about X Y Z
        dx dy dz : displacement  along X Y Z
        rx ry rz : rotation      about X Y Z
.DE
.cs R
All limit specifications are expressed in the
.I tool
frame of the corresponding position equation and
take effect for the subsequent
.I move
request.
To each direction specification must correspond a value, for example :
.br
.cs R 23
.DS L
        limit("fx tz", 10., 5.);
.DE
.cs R
will request a force of 10
.B Newtons
along X, and of a torque 5.
.B Newton-meter
about Z to be monitored.
When either of the specifications
is exceeded, the corresponding motion is interrupted
and the task proceeds with the next request in the motion queue.
The `code' field of the corresponding position is set to `ONF'.
Likewise, distance specifications can be coded as :
.br
.cs R 23
.DS L
        limit("dx ry", 3., 1.);
.DE
.cs R
that causes the motion to be interrupted when the distance
change along X exceeds 3
.B millimeters
or when the rotations about Y exceeds 1
.B degree.
Only absolute values of the limit specifications are taken into account.
.NH 2
Servo Modes, Comply and Lock
.PP
A comply servo mode is requested via the
.B comply
primitive according to the following format :
.br
.cs R 23
.DS L
        comply(dirs, value [, value] ...)
        char *dirs;
        double value;
.DE
.cs R
The
.B comply
primitive causes the subsequent motion request to
be executed such that forces and/or torques are maintained in the
.I tool
frame instead of positions and/or rotations.
The arm then enters the specified
.I comply
mode in the corresponding motion and all the following motions until
the
.B lock
primitive brings the manipulator back into position servo mode for
the selected directions.
The format for
.B lock
is :
.br
.cs R 23
.DS L
        lock(dirs)
        char *dirs;
.DE
.cs R
.PP
The first argument of
.B comply
and
.B lock
is a string containing direction specifications made up of
a combination of the following :
.br
.cs R 23
.DS L
        fx fy fz : force  along X Y Z
        tx ty tz : torque about X Y Z
.DE
.cs R
.PP
The second argument of
.B comply
is the signed magnitude of the desired forces and/or torques.
.PP
Care must be taken that the sequence of servo mode specification is
consistent.
Requiring the arm to comply along or about a direction already in
.I comply
servo mode or locking a direction not in
.I comply
servo mode will cause an error.
In order to keep track of the different specifications, line
indentation is advised :
.br
.cs R 23
.DS L
        move(p0);
        comply("dy", 0.);
                move(p1);
                move(p2);
                comply("dx ry", 5., 3.);
                                move(p3);
                        lock("ry");
                        move(p4);
                        move(p5);
                lock("dx");
                move(p6);
        lock("dy");
        move(p7);
.DE
.cs R
.PP
Either
.I Cartesian
or
.I joint
motion modes can used for complying motion sequences.
However, they behave differently.
In
.I Cartesian
mode, the system automatically compensate for position errors due to
unwanted accommodating joint motions [2].
In
.I joint
mode, there is no compensation.
.NH 2
Carrying Loads
.PP
The function
.B massis
allows the user to specify that a mass is to be carried by the manipulator.
The mass of the object, expressed in kilograms, causes the system
to compute gravity compensation terms in the motions using
force control.
The mass of object is initially set to 0. and can be set or
reset via
.B massis :
.br
.cs R 23
.DS L
        massis(mass)
        double mass;
.DE
.cs R
As usual, the new value is taken into account the next motion request.
.NH 2
Examples
.PP
.B
1)
.R
The first example involves a solid horizontal surface.
The manipulator is programmed to reach to the table
in a motion normal to the expected surface.
It then enters the
.I comply
mode in order to slide along.
During the second sliding motion, it detects an edge of the surface by
exerting a preload force and monitoring the position changes in the
Z direction of the
.I tool
frame.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "umac.h"
     3
     4  pumatask()
     5  {
     6          TRSF_PTR z, e , b1, b2, b3, b4, b5;
     7          POS_PTR  p1, p2, p3, p4, p5;
     8          int q;
     9
    10          z = gentr_trsl("Z",  0.,  0., 864.);
    11          e = gentr_trsl("E" , 0. , 0. , 170.);
    12          b1 = gentr_rot("B1", 600. ,-100.,  300., yunit, 180.);
    13          b2 = gentr_rot("B2", 600. , 200.,  300., yunit, 180.);
    14          b3 = gentr_rot("B3", 600. , 200.,  400., yunit, 180.);
    15          b4 = gentr_rot("B4", 600. ,-100.,  400., yunit, 180.);
    16          b5 = gentr_rot("B5", 500. ,-100.,  300., yunit, 180.);
    17
    18          p1   = makeposition("P1" , z, t6, e, EQ, b1, TL, e);
    19          p2   = makeposition("P2" , z, t6, e, EQ, b2, TL, e);
    20          p3   = makeposition("P3" , z, t6, e, EQ, b3, TL, e);
    21          p4   = makeposition("P4" , z, t6, e, EQ, b4, TL, e);
    22          p5   = makeposition("P5" , z, t6, e, EQ, b5, TL, e);
    23
    24
.DE
.cs R
.br
.cs R 23
.DS L
    25          setmod('c');
    26          setvel(200, 100);
    27          move(p4);
    28          for (; ; ) {
    29                  QUERY(q); if (q == 'n') break;
    30
    31                  p1->end = 0;
    32
    33                  setvel(20, 20);
    34                  limit("fz", 20.);
    35                  move(p1);
    36
    37                  setvel(100, 50);
    38                  comply("fz", 10.);
    39                          move(p2);
    40                  lock("fz");
    41
    42
    43                  waitfor(p1->end);
    44                  if (p1->code != ONF) {
    45                          nextmove = YES;
    46                          printf("where is the table ?\\\\n");
    47                          setvel(200, 100);
    48                          move (park);
    49                          return;
    50                  }
    51
    52                  move(p3);
    53                  move(p4);
    54
    55                  limit("fz", 20.);
    56                  setvel(50, 50);
    57                  move(p1);
    58
    59                  limit("dz", 3.);
    60                  comply("fz", 10.);
    61                          move(p5);
    62                  lock("fz");
    63
    64                  move(p4);
    65
    66                  waitfor(p5->end);
    67                  if (p5->code != OND) {
    68                          printf("where is the edge ?\\\\n");
    69                  }
    70          }
    71          setvel(300, 50);
    72          move(park);
    73  }
.DE
.cs R
.PP
The transforms and positions define a set of five position
next to the surface.
The surface if assumed to be less than 100 millimeters below the
position P4, such that a collision should occur when moving from
P4 to P1, located 100 millimeter below P4.
Position P3 is at the same level than P4 and above P2.
Position P5 is assumed to bring the end effector off the
boundaries of the table, when moving from P1 to P5.
As we may have more motions toward P1 that waits for the end of motion,
the `p1->end' event is reset for each loop line 31.
Lines 33 to 40, implement a sequence of motion requests so as
to program the manipulator to enter the
.I comply
when the obstacle in encountered.
Lines 43 through 50, we make sure that the limit have actually
been met, otherwise the motion toward P2 is canceled as well as the task.
In the normal case, lines 52 and 53 bring the arm back above the surface.
The same guarded motion is then performed toward P1, but now
the motion in
.I comply
servo mode is performed toward P5 where the edge of the surface is
expected to be found.
The termination of this last motion is also checked at lines 66 through 69.
A preload force in the Z direction
is applied for all motions to make sure that
the contact is maintained.
.bp
.PP
.B
2)
.R
In this second example, the manipulator is programmed for the task of
turning a crank.
Two compliant directions are required in this operation.
During the compliant motion, the
.I tool
frame rotates so as to keep a constant
orientation with respect to the crank handle.
We define the compliant directions with respect to this frame.
A grasp position is also defined to allow for some clearance.
The task will turn the crank a given number of times.
One turn is programmed to last 4 seconds.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "umac.h"
     3  #include "hand.h"
     4
     5  int turns;
     6
     7  pumatask()
     8  {
     9          TRSF_PTR z, e, shaft, handle, apro, grasp, rotpx, rotnx;
    10          POS_PTR  get, away;
    11          POS_PTR turn;
    12          int pxfn(), nxfn();
    13          int q;
    14
    15          rotpx = newtrans("ROTPX", pxfn);
    16          rotnx = newtrans("ROTNX", nxfn);
    17          z = gentr_trsl("Z",  0.,  0., 864.);
    18          e = gentr_trsl("E" , 0. , 0. , 170.);
    19          shaft = gentr_trsl("SHAFT", -200., 500., 600.);
    20          shaft->fn = varb;
    21          handle = gentr_trsl("HANDLE", 0., 0., 50.);
    22          apro = gentr_trsl("APRO", -50., 0., 0.);
    23          grasp = gentr_rpy("GRASP", 0., 0., 0., 0., 190., 0.);
    24
    25          get = makeposition(
    26          "GET", z, t6, e, EQ, shaft, handle, grasp, TL, e);
    27
    28          away = makeposition(
    29          "AWAY", z, t6, e, EQ, shaft, handle, grasp, apro, TL, e);
    30
    31          turn = makeposition(
    32          "TURN", z, t6, e, EQ, shaft, rotpx, handle, rotnx, grasp, TL, ro
    33
    34          setvel(300, 300);
    35          move(away);
    36          OPEN;
    37          if (!teach(shaft, get)) {
    38                  move(away);
    39                  move(park);
    40                  return;
    41          }
.DE
.cs R
.br
.cs R 23
.DS L
    42          shaft->fn = const;
    43          optimize(turn);
    44          turns = 4;
    45          waitfor(completed);
    46          CLOSE;
    47          comply("fx fz ", 0., 0.);
    48                  movecart(turn, 200, 4000 * turns);
    49          lock("fx fz ");
    50          move(get);
    51          waitfor(turn->end);
    52          OPEN;
    53          distance("dx", -30.);
    54                  move(get);
    55          setvel(200, 50);
    56          setmod('j');
    57          move(park);
    58  }
    59
    60  pxfn(t)
    61  TRSF_PTR t;
    62  {
    63          Rot(t, xunit, goalpos->scal * 360 * turns);
    64  }
    65
    66  nxfn(t)
    67  TRSF_PTR t;
    68  {
    69          Rot(t, xunit, - goalpos->scal * 360 * turns);
    70  }
.DE
.cs R
.PP
The manipulator is first moved to a safe position away from
obstacles.
Lines 37 to 41, the manual teach mode built in RCCL is called.
This teach mode makes use of the
.B update
function to record a position.
That is why the transform $SHAFT$ is first declared as a
.I varb
transform.
Once this transform is taught, its type can be changed, line 42, and the
position $TURN$ optimized line 43.
Gripper actions are obtained as usual.
Once these preliminary operations are performed, the turning motion
can begin.
It is obtained in terms of a functionally defined motion, line 48,
executed in
.I comply
servo mode.
The duration of the motion is the number of turns times four seconds.
Care has been taken line 32, such that the compliance frame is
properly specified.
.bp
.PP
.B
3)
.R
The third example illustrates the peg in a hole insertion task.
The strategy consists of moving toward the expected location of
the hole with a small approach angle.
Even with a poor position accuracy the end of the peg will enter
the hole with a high degree of confidence.
As soon as a collision occurs, the manipulator is programmed to
go in
.I comply
mode in the Z direction with a preload force in the same direction.
While complying, the peg is rotated so as to be aligned with the axis of
the hole.
The manipulator is then programmed to comply in the normal directions
of the hole axis and
a motion inside the hole is immediately started.
The presence of a small chamfer helps the peg not to slip off the
initial insertion position.
The force in Z is also limited since the insertion may jam
due to a misalignment.
The fit is not very tight, and we can expect that a
portion of the peg is inserted before the jam occurs.
A sequence of four accommodation rotating motions
using
.B update,
allows the
manipulator to ``feel'' the walls of the hole and to record a correct alignment.
In a final effort, the peg is inserted all the way.
Finally, the peg is pulled out with no difficulty since the alignment
has been corrected.
The moment when the task becomes unconstrained  is detected by
monitoring the differential motions.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "umac.h"
     3  #include "hand.h"
     4
     5  pumatask()
     6  {
     7          TRSF_PTR z, e, peg, hole, roty, bottom, angle;
     8          POS_PTR align, in, touch;
     9          int q;
    10
    11          z = gentr_trsl("Z",  0.,  0., 864.);
    12          e = gentr_trsl("E" , 0. , 0. , 140.);
    13          peg = gentr_trsl("PEG", 0., 0., 10.);
    14          hole = gentr_trsl("HOLE", -50., 450., 500.);
    15          hole->fn = varb;
    16          bottom = gentr_trsl("BOTTOM", 0., 0., -20.);
    17          roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
    18          angle = gentr_rot("ANGLE", 0., 0., 0., yunit, 12.);
    19
    20          align = makeposition(
    21          "ALIGN", z, t6, e, peg, EQ, hole, roty, TL, peg);
    22
    23          touch = makeposition(
    24          "TOUCH", z, t6, e, peg, EQ, hole, angle, roty, TL, peg);
    25
    26          in = makeposition(
    27          "IN", z, t6, e, peg, EQ, hole, bottom, roty, TL, peg);
    28
    29          setvel(300, 50);
    30          move(align);
    31          if (!teach(hole, align)) {
    32                  setvel(300, 50);
    33
    34                  distance("dz", -100.);
    35                          move(align);
    36
    37                  setmod('j');
    38                  move(park);
    39                  return;
    40          }
    41          setmod('c');
    42          setvel(100, 100);
    43
    44          distance("dz", -10.);
    45                  move(touch);
    46
    47          QUERY(q);
    48
    49          if (q == 'n') {
    50                  setvel(300, 100);
    51                  setmod('j');
    52                  move(park);
    53                  return;
    54          }
.DE
.cs R
.br
.cs R 23
.DS L
    57          setvel(4, 7);
    58          distance("dz", -4.);
    59                  move(touch);
    60
    61          limit("fz", 25.);
    62          distance("dz", 5.);
    63                  move(touch);
    64
    65          comply("fz", 15.);
    66                  move(align);
    67          lock("fz");
    68
    69          comply("fx fy", 0., 0.);
    70                  limit("fz", 20.);
    71                  move(in);
    72
    73                  update(hole, in);
    74                  limit("fz tx", 40., 10.);
    75                  distance("rx", 2.);
    76                          move(in);
    77
    78                  update(hole, in);
    79                  limit("fz tx", 40., 10.);
    80                  distance("rx", -2.);
    81                          move(in);
    82
    83                  update(hole, in);
    84                  limit("fz ty", 40., 10.);
    85                  distance("ry", 2.);
    86                          move(in);
    87
    88                  update(hole, in);
    89                  limit("fz ty", 40., 10.);
    90                  distance("ry", -2.);
    91                          move(in);
    92
    93                  update(hole, in);
    94                  limit("fz", 20.);
    95                  distance("dz", 10.);
    96                          move(in);
    97
    98                  limit("dx dy", 1., 1.);
    99                  move(align);
   100          lock("fx fy");
   101
   102          setvel(50, 50);
   103          distance("dz", -50.);
   104                  move(align);
   105
   106          setvel(300, 50);
   107          setmod('j');
   108          move(park);
   109  }
.DE
.cs R
.PP
The beginning of this task is quite similar to the previous example and
also makes use of the manual teach mode to record an approximate
initial position.
Lines 44 and 45, the manipulator moves to an approach position
and a chance is given to the user to cancel the task.
Line 57 to 63, an approach motion and a purposely over shooting motion
is programmed in order to obtain the initial phase of the insertion
process.
While complying and exerting a preloading force the peg is rotated,
lines 65 to 67, to the aligned position.
The first insertion attempt is performed line 70 and 71.
Lines 70 to 91, are programmed the accommodation motions using
.B update
to record the successive alignments.
The final phase of the insertion process is performed lines at 93 to 96.
The peg is then pulled out of the hole, while monitoring the
differential motions signaling that the motion becomes unconstrained.
The peg is then taken away lines 102 to 104.
.bp
.PP
.B
4)
.R
The last example demonstrates how compliant degrees of freedom
can be accumulated as constraints are met.
The manipulator is programmed to detect the walls of a corner and to
record the position of the corner.
The program then uses this position information to move the manipulator
next to the corner within a very small distance.
.br
.cs R 23
.DS L
     1  #include "rccl.h"
     2  #include "umac.h"
     3
     4  pumatask()
     5  {
     6          TRSF_PTR z, e, peg, corner, roty;
     7          POS_PTR pcor;
     8          int q;
     9
    10          z = gentr_trsl("Z",  0.,  0., 864.);
    11          e = gentr_trsl("E" , 0. , 0. , 140.);
    12          peg = gentr_trsl("PEG", 0., 0., 10.);
    13          corner = gentr_trsl("CORNER", -50., 500., 550.);
    14          corner->fn = varb;
    15          roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
    16
    17          pcor = makeposition(
    18          "PCOR", z, t6, e, peg, EQ, corner, roty, TL, peg);
    19
    20          setvel(300, 50);
    21          move(pcor);
    22          if (!teach(corner, pcor)) {
    23                  setvel(300, 50);
    24
    25                  setmod('j');
    26                  move(park);
    27                  return;
    28          }
    29          setmod('c');
    30          setvel(100, 100);
    31
    32          distance("dz", -50.);
    33                  move(pcor);
    34
    35          QUERY(q);
    36
    37          if (q == 'n') {
    38                  setvel(300, 100);
    39                  setmod('j');
    40                  move(park);
    41                  return;
    42          }
    43          move(pcor);
    44
    45          setvel(5, 20);
.DE
.cs R
.br
.cs R 23
.DS L
    46
    47          limit("fz", 20.);
    48          distance("dz", 10.);
    49                  move(pcor);
    50          comply("fz", 10.);
    51                  limit("fy", 15.);
    52                  distance("dy", -10.);
    53                          move(pcor);
    54                  comply("fy", -10.);
    55                          update(corner, pcor);
    56                          limit("fx", 25.);
    57                          distance("dx", 10.);
    58                                  move(pcor);
    59          lock("fz fy");
    60          setvel(50, 50);
    61          distance("dx dy dz", -10., 10., -50.);
    62                  move(pcor);
    63
    64          setvel(300, 50);
    65          setmod('j');
    66          move(park);
    67          distance("dx dy dz", -10., 10., -50.);
    68                  move(pcor);
    69          setmod('c');
    70          setvel(50, 50);
    71          distance("dx dy dz", -1., 1., -1.);
    72                  move(pcor);
    73          distance("dx dy dz", -10., 10., -50.);
    74                  move(pcor);
    75          setvel(300, 50);
    76          setmod('j');
    77          move(park);
    78  }
.DE
.cs R
.PP
Again the preliminary phase is quite similar to the previous
example.
The approximate location of the corner is taught by an operator and
a chance is also given, line 32 to 43, to cancel the task.
The reader may notice that in this example, the corner is oriented
in such a way that approaching it corresponds to positive
displacements in the X and Z directions, and a negative one in the Y
direction.
The manipulator approaches the first wall of the corner moving along the
Z direction, lines 48 and 49, and enters the first
.I comply
mode, line 50, before moving to the next wall.
The same process is repeated for the Y and Z directions.
In each case a preload force is exerted in the appropriate direction
in order to maintain contact with the walls.
The last accommodation motion, line 58, is associated to a call
to
.B update
so as to record the final position.
Two compliance degrees of freedom are accumulated and the
manipulator is brought back to position servo mode line 59.
The peg is then taken away, lines 60 to 62.
Before going back to the recorded position, the arm is
moved at high velocity to the PARK position.