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

Text File | 1987-03-02 | 24.8 KB | 930 lines

.ND
.EQ
delim $$
.EN
.nr H1 3
.NH 1
Basic Components : Numbers, Vectors, Transformations, Differential motions, Forc
.PP
We shall now describe in more detail the meaning and form of a first set of
RCCL primitives and how they can be used in manipulator programs.
.SH
REMARKS
.PP
All RCCL functions returning a structure, follow the
convention that the result is the left argument (output argument)
and that first argument is returned as the
.B value
of the function (in the same style as
.B strcat
does).
This allows to code in the following style :
.br
.cs R 23
.DS L
        trans = rot(newtrans("TRANS", const), zunit, 90.);
.DE
.cs R
which in one line, allocates a transform and sets it to a pure rotation
around the Z direction.
Because the type of each function is declared in the file
.B rccl.h
, the program lint will complain if the returned value is not used.
Each function of this style is associated with a macro that
capitalizes the first letter.
In case of `rot', the macro is :
.br
.cs R 23
.DS L
#define Rot     (void) rot
.DE
.cs R
such that the same above code can be written as:
.br
.cs R 23
.DS L
        trans = newtrans("TRANS", const);
        Rot(trans, zunit, 90.);
.DE
.cs R
without complains from lint.
.NH 2
Numbers
.PP
The
.B rccl.h
include file contains structured definitions of vectors
and transformations that
should be used in connection with the corresponding functions.
These structure declarations are preceded with C `typedef' definitions that
better describe the implementations of basic data types :
.br
.cs R 23
.DS L
typedef int bool;

typedef float real;
.DE
.cs R
C knows two floating point variable types : double and float.
They correspond on most machines to single and double precision floating
point representation and arithmetic.
For efficiency, all calculations are performed in single precision.
In order to insure consistency throughout the RCCL code, the type `real'
has been declared as a C typedef.
Every single floating point variable
is declared as such.
Because C structures are always passed by address, and
because `double' and `float' variables have different sizes,
the proper address calculations are insured.
However, automatic type conversions will give meaningful results
if type `double' variables are assigned to or from RCCL variables.
.PP
A set of math constant global variables is included in the library :
.br
.cs R 23
.DS L
        real pi_m      is    PI
        real pib2_m    is    PI / 2
        real pit2_m    is    PI * 2
        real dgtord_m  is    PI / 180
        real rdtodg_m  is    180 / PI
.DE
.cs R
.PP
The purpose of those variables is to avoid a unnecessary
increase of the size of process data region (see end(2)) and
they are initialized at compile time.
Setting them to any other values guarantees unpredictable results.
.NH 2
Vectors
.PP
The type `vector' is described by the following structure :
.br
.cs R 23
.DS L
typedef struct vector {
                real x, y, z;
} VECT, *VECT_PTR;
.DE
.cs R
The C `typedef' feature is a way of giving another name to basic data
types.
.PP
A C structure variable `k' implementing a vector can either be coded as:
.br
.cs R 23
.DS L
        struct vector k;
or
        VECT k;
.DE
.cs R
.PP
A pointer to a vector variable can either be coded as :
.br
.cs R 23
.DS L
        struct vector *pk;
or
        VECT *pk;
or
        VECT_PTR pk;
.DE
.cs R
The choice is according to taste and coding habits.
Using this structure gives access to the following functions:
.B dot,
.B assignvect,
.B cross,
and
.B unit.
In order to describe the argument types of these functions
and the type of the value that they return,
their heading declarations are displayed :
.br
.cs R 23
.DS L
        real dot(u, v)
        VECT_PTR u, v;

        VECT_PTR assignvect(v, u)
        VECT_PTR v, u;

        VECT_PTR cross(r, u, v)
        VECT_PTR r, u, v;

        VECT_PTR unit(v, u)
        VECT_PTR v, u;
.DE
.cs R
The function
.B dot
returns the dot product of two vectors.
The function
.B assignvect
copies its second argument into the first one.
Likewise, the function
.B cross
returns in its left argument the cross product of the two remaining
arguments.
The function
.B unit
computes a vector collinear with its right argument vector
but of unit magnitude.
Taking the cross product of two identical vectors is meaningless.
By contrast, the
.B unit
function can perfectly take two identical arguments.
In that case, the magnitude of the vector would be set to unity `in place'.
.NH 2
Transformations
.PP
The corresponding C structure is :
.br
.cs R 23
.DS L
typedef int(* TRFN)();

typedef struct transform {
                char *name;
                TRFN fn;
                VECT n, o, a, p;
                int timeval;
} TRSF, *TRSF_PTR;
.DE
.cs R
The first entry in the structure is a pointer to a string that stands
for the transform name.
The second, is a pointer to a function.
The function pointer can be set to one of the user's background
real-time function
or to one of the system functions
.B const,
.B varb,
or
.B hold.
A more complete discussion of this point occurs later.
For now, we can assume that this pointer will most
of the time point to the
.B const
function, meaning that the transform is constant transformation
and will not change throughout the execution of the task.
The next entry contains the value of the transform itself built in terms
of four vectors: the
.B normal,
.B orientation,
.B approach,
and
.B position
vectors.
The last row the transform is assumed to be : {0 0 0 1}.
In other words, the transforms can only be orthogonal transforms.
The last entry is the time of the last evaluation of the function,
needed in the case of functionally described transforms.
.PP
This type declaration gives access to the following functions :
.br
.cs R 23
.DS L
        TRSF_PTR assigntr(t1, t2)
        TRSF_PTR t1, t2;

        TRSF_PTR taketrsl(t1, t2)
        TRSF_PTR t1, t2;

        TRSF_PTR takerot(t1, t2)
        TRSF_PTR t1, t2;

        TRSF_PTR trmult(r, t1, t2)
        TRSF_PTR r, t1, t2;

        TRSF_PTR trmultinp(r, t)
        TRSF_PTR r, t;

        TRSF_PTR trmultinv(r, t)
        TRSF_PTR r, t;

        TRSF_PTR invert(r, t)
        TRSF_PTR r, t;

        TRSF_PTR invertinp(t)
        TRSF_PTR t;
.DE
.cs R
The
.B assigntr
function is quite similar to the
.B assignvect
function above and the same remarks can be made.
It must, however, be noticed that only the value part of the transform
is copied and not the other components of the structure.
The functions
.B taketrsl
and
.B takerot
perform a selective copy of the translational (resp. rotational) part,
and leaves untouched the rotational (resp. translational) part.
The function
.B trmult
multiplies the two right arguments transforms and leaves the
result in the left argument.
This function requires the three arguments to be different.
The function
.B trmultinp
multiplies the two arguments and leaves the result in the
left argument.
The function
.B trmultinv
multiplies the left argument by the inverse of the right one and leave the
result in the left argument.
The function
.B invert
leaves in the left argument the inverse the right one.
Since the arguments must be different, the function
.B invertinp
performs an inversion `in place'.
.PP
The following functions selectively set the terms of the transformations :
.br
.cs R 23
.DS L
        TRSF_PTR trsl(t, px, py, pz)
        TRSF_PTR t;
        real px, py, pz;

        TRSF_PTR vao(t, ax, ay, az, ox, oy, oz)
        TRSF_PTR t;
        real ax, ay, az, ox, oy, oz;

        TRSF_PTR rot(t, k, h)
        TRSF_PTR t;
        VECT_PTR k;
        real h;

        TRSF_PTR eul(t, phi, the, psi)
        TRSF_PTR t;
        real phi, the, psi;

        TRSF_PTR rpy(t, phi, the, psi)
        TRSF_PTR t;
        real phi, the, psi;
.DE
.cs R
All these functions use a transformation pointer as left argument, which
as usual is returned as a value of the function.
The function
.B trsl
sets the terms of the $p$ vector of the transformation and leaves
the rotational part untouched.
All distances in RCCL are expressed in millimeters.
The function
.B vao
sets the vectors $n$, $o$, and $a$ of the transformation.
Since the vectors $n$, $o$ and $a$ are orthogonal,
.B vao
only needs the terms of $o$ and $a$ and builds the vector $n$.
The vectors whose components are passed as arguments do not need to
be orthogonal.
The rotational part of the transform is built as follows: take the
user's supplied $a$ vector, normalize it and use it as the final $a$ vector,
take
the user's supplied $o$ vector (which may not be orthogonal) and
build a possibly non unit vector $n$ but orthogonal with $o$ and $a$,
reconstruct $o$ as to be orthogonal with $n$ and $a$,
normalize it, and finally
derive $n$ from $o$ and $a$.
The function
.B rot
sets the rotational part of the transformation as a rotation around
a vector possibly unnormalized, second argument,
of a given angle, third argument, expressed in
degrees.
The function
.B eul
sets the rotational part of the transformation as a rotation expressed
with Euler angles in degrees.
Finally, the function
.B rpy
sets the rotational part of the transformation as a rotation expressed
with roll, pitch, and yaw angles in degrees.
These rotation setting functions leaves the translational part
of the transform untouched.
.PP
The next set of functions are similar in form to the previous ones, except
that the transform, left argument, is multiplied by a translation
or a rotation (which is quite a different thing).
As usual, the left argument transformation pointer is returned as value of the
function.
.br
.cs R 23
.DS L
        TRSF_PTR trslm(t, px, py, pz)
        TRSF_PTR t;
        real px, py, pz;

        TRSF_PTR vaom(t, ax, ay, az, ox, oy, oz)
        TRSF_PTR t;
        real ax, ay, az, ox, oy, oz;

        TRSF_PTR rotm(t, k, h)
        TRSF_PTR t;
        VECT_PTR k;
        real h;

        TRSF_PTR eulm(t, phi, the, psi)
        TRSF_PTR t;
        real phi, the, psi;

        TRSF_PTR rpym(t, phi, the, psi)
        TRSF_PTR t;
        real phi, the, psi;
.DE
.cs R
As stated at the beginning of this section, when the value of the function
is unwanted, a set a macros is provided.
They produce the following list
of names :
.br
.cs R 23
.DS L
Assignvect      Cross           Unit
Assigntr        Taketrsl        Takerot
Trmult          Trmulinp        Trmultinv
Invert          Invertinp
Trsl            Vao             Rot             Eul            Rpy
Trslm           Vaom            Rotm            Eulm           Rpym
.DE
.cs R
As we are able to specify the rotational part of transforms with
Euler or roll, pitch, yaw angles, we may need to derive them from
a given transformation.
These representations are not unique for
a given rotation.
The functions are :
.br
.cs R 23
.DS L
        noatoeul(phi, the, psi, t)
        real *phi, *the, *psi;
        TRSF_PTR t;

        noatorpy(phi, the, psi, t)
        real *phi, *the, *psi;
        TRSF_PTR t;
.DE
.cs R
Please note that the three first arguments are
.B pointers
to the three results of the pseudo type `real'.
.PP
We now need to use transformation as easily as
we would use simple data types in C.
At the beginning of manipulation functions, one needs
to declare transformations and to allocate memory for them.
This can be done in the following manner :
.br
.cs R 23
.DS L
pumatask()
{
        TRSF base;

        ...
        ...
}
.DE
.cs R
This way of allocating memory for transformations presents three major
drawbacks.
The first one is that dynamic variables, allocated in the
stack, only live the duration of the function call.
Since the execution of manipulator programs is not explicitly synchronized
with the calculation of trajectories,
the function may well exit before the requested motions
are completed.
All the memory space allocated in the stack would be allocated for
other purposes.
This will surely cause a lot a trouble because the values of the
transformations are used for the trajectory calculations.
One may go around this by writing :
.br
.cs R 23
.DS L
        static TRSF base;
.DE
.cs R
but the space would remain permanently allocated.
The second trouble is that the value of the transforms and other
entries in the structure need to be initialized.
If one chooses to use dynamic stack allocations,
one also need to synchronize the function such as it does not exit
before the transforms are no longer in use :
.br
.cs R 23
.DS L
pumatask()
{
        TRSF base;

        base.name = "NAME";             /* set the name */
        base.fn = const;                /* tell it's constant */
        Assigntr(&base, unitr);         /* init to unit transform */
        Trsl(&base, 0., 0., 200.);      /* set it to a translation */
        base.timeval = 0;               /* reset time eval */

        ...

        waitfor(completed)              /* make sure not any more in use */
}
.DE
.cs R
The third drawback is that we will most of the time refer to transforms
by pointers, and it would lead to a heavy use the the `&' operator.
The initialization statement for a static `TRSF' variable would not be
any more convenient and would be very error prone:
.br
.cs R 23
.DS L
        static TRSF base = {"BASE",
                            const,
                            1.,0.,0.,
                            0.,1.,0.,
                            0.,0.,1.,
                            0.,0.,200.
                            0,
                           };
.DE
.cs R
.PP
Although the techniques described above are perfectly viable,
RCCL provides a built-in dynamic memory allocation
system for transforms (and positions).
The basic call is the function
.B newtrans :
.br
.cs R 23
.DS L
        TRSF_PTR newtrans(n, fn)
        char *n, TRFN fn;
.DE
.cs R
This function returns a pointer to a transform initialized to the
unit transform.
The second argument is a pointer to a function, either one
of the user's functions which ,as we will see,
have to possess certain properties,
or one of the predefined functions
.B
const, varb, hold.
.R
Since
.B newtrans
dynamically allocate memory in user space,
the creation of too many transforms will
cause a program exit with the message ``mem. alloc error''.
The statement :
.br
.cs R 23
.DS L
        freetrans(t);
.DE
.cs R
permits the system to free the allocated memory when needed (it is implemented
as a macro).
.PP
Other RCCL functions make use of
.B newtrans
as a short hand for common coding patterns:
.br
.cs R 23
.DS L
        TRSF_PTR gentr_trsl(name, px, py, pz)
        char *name;
        real px, py, pz;

        TRSF_PTR gentr_rot(name, px, py, pz, k, h)
        char *name;
        real px, py, pz, h;
        VECT_PTR k;

        TRSF_PTR gentr_pao(name, px, py, pz, ax, ay, az, ox, oy, oz)
        char *name;
        real px, py, pz, ax, ay, az, ox, oy, oz;

        TRSF_PTR gentr_eul(name, px, py, pz, phi, the, psi)
        char *name;
        real px, py, pz, phi, the, psi;

        TRSF_PTR gentr_rpy(name, px, py, pz, phi, the, psi)
        char *name;
        real px, py, pz, phi, the, psi;
.DE
.cs R
These functions permit us to create transformations and initialize
them all at once.
They all return a pointer to the created transforms
by default set as
.B const
transforms.
The first four arguments are : the name (string), and the components of
the $p$ vector.
For creating transforms containing non unit rotations,
the expression of the rotational part is analogous to the previous
family of functions.
For example :
.br
.cs R 23
.DS L
        TRSF_PTR t1, t2, t3;    /* declare transform pointers */

        ...

        t1 = trsl(eul(newtrans("T1", const), 10., 20., 30.), 1., 2., 3.);

        t2 = gentr_eul("T2", 1., 2., 3., 10., 20., 30.);

        t3 = gentr_trsl("T3", 1., 2., 3.);
        Eul(t3, 10., 20., 30);
.DE
.cs R
give three identical transforms.
.PP
The last group of transformation related functions are for output :
.br
.cs R 23
.DS L
        printr(t, fp)
        TRSF_PTR t;
        FILE *fp;

        printe(e, fp)
        TRSF_PTR e;
        FILE *fp;

        printy(e, fp)
        TRSF_PTR e;
        FILE *fp;

        printrn(t, fp)
        TRSF_PTR t;
        FILE *fp;
.DE
.cs R
The function
.B printr
prints the numerical value of the transform, first argument.
The functions
.B printe
and
.B printy
respectively print the Euler and pith, roll, yaw angles.
The function
.B printrn
prints the name, the numerical value and the angles altogether.
All these functions take as a second argument a UNIX file pointer.
As an example, the output of the following sequence of calls :
.br
.cs R 23
.DS L
        TRSF_PTR t1, t2, t3;

        t1 = gentr_eul("T1", 10., 20., 30., 11., 12., 13.);
        printf("part 1\\\\n");
        printe(t1, stdout);
        printrn(t1, stdout);

        t2 = newtrans("T2", const);
        printf("part 2\\\\n");
        printr(t2, stdout);
        Rot(t2, yunit, 90.);
        Trslm(t2, 10., 20., 30.);
        printrn(t2, stdout);

        t3 = newtrans("T3", const);
        printf("part 3\\\\n");
        printrn(trmult(t3, t1, t2), stdout);

        printf("part 4\\\\n");
        printrn(trmult(t3, t2, t1), stdout);
.DE
.cs R
would be :
.br
.cs R 23
.DS L
part 1
EUL x:10.000  y:20.000  z:30.000  phi:11.000  the:12.000  psi:13.000
T1 :
   0.893   -0.402    0.204   10.000
   0.403    0.914    0.040   20.000
  -0.203    0.047    0.978   30.000
EUL x:10.000  y:20.000  z:30.000  phi:11.000  the:12.000  psi:13.000
RPY x:10.000  y:20.000  z:30.000  phi:24.280  the:11.688  psi:2.738
part 2
   1.000    0.000    0.000    0.000
   0.000    1.000    0.000    0.000
   0.000    0.000    1.000    0.000
T2 :
   0.000    0.000    1.000   30.000
   0.000    1.000    0.000   20.000
  -1.000    0.000    0.000  -10.000
EUL x:30.000  y:20.000  z:-10.000 phi:0.000   the:90.000  psi:0.000
RPY x:30.000  y:20.000  z:-10.000 phi:0.000   the:90.000  psi:0.000
part 3
T3 :
  -0.204   -0.402    0.893   26.700
  -0.040    0.914    0.403   49.973
  -0.978    0.047   -0.203   15.076
EUL x:26.700  y:49.973  z:15.076  phi:24.280  the:101.688 psi:2.738
RPY x:26.700  y:49.973  z:15.076  phi:-169.000 the:78.000  psi:167.000
part 4
T3 :
  -0.203    0.047    0.978   60.000
   0.403    0.914    0.040   40.000
  -0.893    0.402   -0.204  -20.000
EUL x:60.000  y:40.000  z:-20.000 phi:2.323   the:101.776 psi:24.240
RPY x:60.000  y:40.000  z:-20.000 phi:116.707 the:63.207  psi:116.922
.DE
.cs R
This should also suffice to remind us that the matrix product (and also
orthogonal transforms products) is not commutative.
.NH 2
Differential Motions and Forces.
.PP
Although RCCL do not explicitly use the structured representation of
differential motions
or generalized forces
in manipulation primitive
calls, they are made available to the user.
A differential motion is expressed in terms of a differential
translation vector and differential rotation vector.
A generalized force is expressed in terms of a linear force vector
and a moment vector.
The corresponding structures are :
.br
.cs R 23
.DS L
typedef struct diff {
                VECT t, r;
} DIFF, *DIFF_PTR;

typedef struct force {
                VECT f, m;
} FORCE, *FORCE_PTR;
.DE
.cs R
The associated functions are :
.br
.cs R 23
.DS L
        DIFF_PTR assigndiff(t, o)
    DIFF_PTR t, o;

        TRSF_PTR df_to_tr(t, d)
    TRSF_PTR t;
    DIFF_PTR d;

        DIFF_PTR tr_to_df(d, t)
    DIFF_PTR d;
    TRSF_PTR t;

        DIFF_PTR difftr(dt, d, tr)
    DIFF_PTR dt, d;
    TRSF_PTR tr;

        printd(d, fp)
        DIFF_PTR d;
        FILE *fp;

        FORCE_PTR assignforce(t, o)
    FORCE_PTR t, o;

        FORCE_PTR forcetr(ft, f, tr)
    FORCE_PTR ft, f;
    TRSF_PTR tr;

        printm(d, fp)
        FORCE_PTR d;
        FILE *fp;
.DE
.cs R
The function
.B assigndiff
performs a copy of a differential motion structure.
The function
.B df_to_tr
builds a transformation out of a differential motion.
The function
.B tr_to_df
builds a differential motion structure, given a transformation.
The function
.B difftr
transforms a differential motion expressed with respect
to one frame into the same differential motion expressed with respect to
another frame.
For example if $P1$ if a frame expressed in base coordinates
and $P2$ its transformation by $T$ such as :
.EQ
~P2~=~T~P1
.EN
A differential motion expressed with respect
to P1, is obtained expressed with respect
to $P2$.
The left argument of
.B difftr
is the output argument : the transformed differential motion,
the second argument is the original differential motion and the
third argument is the transform expressing the differential relationship.
The
.B prind
function prints on one line a differential motion.
.PP
The functions
.B
assignforce, forcetr, printm
.R
perform analogous processing of generalized forces and torques.
Note that if the forces are expressed in Newtons, torques must be expressed
in Newton-millimeters since distances are in millimeters, the conversions
are straightforward.
As for other functions of that kind the following name can be used instead,
if the returned pointer is not used :
.br
.cs R 23
.DS L
Assigndiff      Difftr          Df_to_tr        Tr_to_df
Assignforce     Forcetr
.DE
.cs R
For example, the following sequence of program statements :
.br
.cs R 23
.DS L
        DIFF  Dp1, Dp2;
        FORCE Fp1, Fp2;
        TRSF_PTR t = gentr_pao("T", 10., 5., 0., 1., 0., 0., 0., 0., 1.);

        printrn(t, stdout);

        Dp2.t.x = 1.;
        Dp2.t.y = 0.;
        Dp2.t.z = .5;
        Dp2.r.x = 0.;
        Dp2.r.y = .1;
        Dp2.r.z = 0.;
        printd(&Dp2, stdout);
        printd(difftr(&Dp1, &Dp2, t), stdout);

        Fp2.f.x = 10.;
        Fp2.f.y = 0.;
        Fp2.f.z = 0.;
        Fp2.m.x = 0.;
        Fp2.m.y = 100.;
        Fp2.m.z = 0.;
        printm(&Fp2, stdout);
        printm(forcetr(&Fp1, &Fp2, t), stdout);
.DE
.cs R
will produce the following output :
.br
.cs R 23
.DS L
T :
   0.000    0.000    1.000   10.000
   1.000    0.000    0.000    5.000
   0.000    1.000    0.000    0.000
EUL x:10.000  y:5.000   z:0.000   phi:0.000   the:90.000  psi:90.000
RPY x:10.000  y:5.000   z:0.000   phi:90.000  the:0.000   psi:90.000
tx  1.0e+00  ty  0.0e+00  tz  5.0e-01   rx  0.0e+00 ry  1.0e-01 rz  0.0e+00
tx  0.0e+00  ty -5.0e-01  tz  1.0e+00   rx  1.0e-01 ry  0.0e+00 rz  0.0e+00
fx  1.0e+01  fy  0.0e+00  fz  0.0e+00   mx  0.0e+00 my  1.0e+02 mz  0.0e+00
fx  0.0e+00  fy  0.0e+00  fz  1.0e+01   mx  1.0e+02 my  5.0e+01 mz  0.0e+00
.DE
.cs R
.NH 2
Events
.PP
RCCL uses the notion of
.I event
to synchronize the user's program with the manipulator motions.
.I
Motion requests
.R
are entered into a queue at a given moment, and executed
on the basis of the first in, first out,
when
all the previous request are served.
The first snare one can run into is depicted by the following :
.br
.cs R 23
.DS L
        for (i = 0; i < 10000; ++i) {
                move(p1);
                move(p2);
        }
.DE
.cs R
The almost `infinite' loop being asynchronously executed, the queue
will be become saturated in a few milliseconds.
In this situation, we have chosen to cause an error condition since
it will most of the time be the result of a program flaw.
In many occasions an
.I event
will be needed to explicitly synchronize
the program with the arm motions, say for opening and closing a gripper.
.KS
.PP
An
.I event,
is defined in RCCL as an integer :
.br
.cs R 23
.DS L
typedef int event;
.DE
.cs R
An event is essentially a count, if positive, it represents the number
of processes waiting for the occurrence.
Occurrence of an event decreases the count by one, when the count
drops to zero, no process is waiting for it.
RCCL maintains the built in event
.B completed
that occurs when the motion queue becomes empty.
The user's program may use the primitive
.B waitfor
implemented as a macro, to synchronize with events, for example :
.KE
.br
.cs R 23
.DS L
        move(p1);
        move(p2);
        move(p3);
        waitfor(completed)
        printf("the arm has reached 'p3', proceeding...\\\\n");
.DE
.cs R
or else, using the
.I event
called `end' associated with each position :
.br
.cs R 23
.DS L
        move(p1);
        move(p2);
        move(p1);
        move(p2);
        OPEN;
        waitfor(p1->end)
        CLOSE;
        waitfor(p2->end)
        OPEN;
        waitfor(p1->end)
        CLOSE;
        waitfor(p2->end)
        OPEN;
.DE
.cs R
to realize synchronization of gripper actions.