\input rccluginit.latex

\chapter{Basic Data Types}

We shall now describe in detail some of the RCCL data structures, in
particular vectors, transformations, differential motions, and forces.
We shall also indicate how they can be used in manipulator programs.

\section{The Data Type Functions}

Each RCCL data type is generally associated with a cluster of functions for
modifying instances of that type, or converting it to another type.
(It is in these cases that RCCL would most greatly benefit from being
implemented in a more ``object oriented'' language, such as
C++; if anyone decides to do this, the authors would
be interested in knowing.)

The data type functions
follow a parameter passing
convention
where (1) a pointer to the type being modified is passed in
as the leftmost
argument, and (2) the function returns the value of this
pointer (in the same style as the the string manipulation
functions described in
{\tt string(3)}.)
This permits calls to be nested in the following way:
\begin{verbatim}
        trans2 = rotToTrsf (xyzToTrsf (trans1, 1.0,2.0,3.0), zunit, 90.0);
\end{verbatim}
which, in one line, sets the translational components of 
a transform to 1.0, 2.0, and 3.0, 
and then sets the rotational component to describe a turn of
$90^\circ$ about $z$.

Structure arguments are passed by reference to save execution time.

Most of the special data types are defined in the file
{\tt <robotTypes.h>}, which is included automatically by {\tt <rccl.h>}.

\section{Some Basic Definitions}

The include file {\tt <rccl.h>} references {\tt <cdefs.h>}, which defines
a few very basic things used by most of the system:
\begin{verbatim}
        #define NULL    0
        #define NULLF   (int(*)())0

        #define YES     1
        #define NO      0
        #define UNDEF   (-1)

        #define MIN(x,y)        ((x) < (y) ? (x) : (y))
        #define MAX(x,y)        ((x) > (y) ? (x) : (y))
        #define ABS(x)          ((x) < 0 ? -(x) : (x))
        #define NINT(x)         ((int)(((x)<0) ? ((x)-0.5)) : ((x)+0.5))

        #define ARRAY_SIZE(a)   (sizeof(a)/sizeof(a[0]))
\end{verbatim}
{\tt NULL} and {\tt NULLF} are used for identifying null pointers,
{\tt YES} and {\tt NO} are ``user friendly'' booleans,
{\tt UNDEF} is used to indicate that some (normally non-negative)
quantity is undefined,
{\tt MAX()}, {\tt MIN()}, {\tt ABS()}, and {\tt NINT()} are obvious,
and {\tt ARRAY_SIZE()} is used for automatically determining the number of
elements in an array.

There are some other things in {\tt <cdefs.h>}, but they won't be
referred to in this manual.

\section{Numbers}

The file {\tt <rccl.h>} also references {\tt <robotMath.h>},
which defines several numeric constants:
\begin{verbatim}
        PI
        PIB2
        PIT2
        DEGTORAD
        RADTODEG
\end{verbatim}
These are, respectively, $\pi$, $\pi/2$, $2 \pi$, $\pi / 180$,
and $180 / \pi$.
%\index {\tt PI}{}
%\index {\tt PIB2}{}
%\index {\tt PIT2}{}
%\index {\tt DEGTORAD}{}
%\index {\tt RADTODEG}{}

\section{Units}

Distances in RCCL are expressed in millimeters. Rotational units
are expressed in either degrees or radians; there is some occasional
inconsistency, which is historical in origin but can be
a bit bothersome. Rotations are always expressed {\it internally\/}
in radians, and functions which accept (or output) degrees convert
to (or from) the radian representation at the highest level.

Force is expressed in Newtons and torque is expressed
in Newton-millimeters.

Time is expressed in milliseconds.

\section{Vectors}

Spatial 3-vectors are described by the type {\tt VECT}:
%\index {vectors}{}
\begin{verbatim}
        typedef struct {
                float x, y, z;
        } VECT, *VECT_PTR;
\end{verbatim}

A pointer to a vector variable can either be defined as
{\tt VECT *pk} or {\tt VECT_PTR pk}, 
depending on the programmer's taste and coding style.
This paradigm will be followed with the other data types.

Vectors are assigned and manipulated by the following functions:
\begin{verbatim}
        float dotVect (v1, v2)
          VECT_PTR v1, v2;

        VECT_PTR crossVect (vr, v1, v2)
          VECT_PTR vr, v1, v2;

        VECT_PTR unitVect (vr, v1)
          VECT_PTR vr, v1;
\end{verbatim}
The function
{\tt dotVect()}
%\index {\tt dotVect()}{}
returns the dot product of two vectors.
The function
{\tt crossVect()}
%\index {\tt crossVect()}{}
computes the cross product of {\tt v1} with {\tt v2}, and returns
the result in {\tt vr}.
The function
{\tt unitVect()}
%\index {\tt unitVect()}{}
computes a unit vector parallel to {\tt v1}
and returns this in {\tt vr}.

For convenience, 
RCCL pre-defines the three {\tt VECT_PTR} types {\tt xunit}, {\tt yunit}, and 
{\tt zunit}, and sets them to point to unit vectors along the $x$, $y$, and
$z$ axes.

\section{Transformations}
%\index {transformations}{}

A $4 \times 4$ homogeneous transformation is represented by the structure
{\tt TRSF}:
\begin{verbatim}
        typedef struct {
                VECT n, o, a, p;
        } TRSF, *TRSF_PTR;
\end{verbatim}
The entries in the structure are simply the columns of the
matrix which comprise the
{\it normal},
%\index {normal vector}{}
{\it orientation},
%\index {orientation vector}{}
{\it approach},
%\index {approach vector}{}
and
{\it position}
%\index {position vector}{}
vectors ({\bf n}, {\bf o}, {\bf a}, and {\bf p}).
The last row of
the transform is assumed to be $[0, 0, 0, 1]$,
so these types contain enough information
to do only three dimensional coordinate 
transformations, not scaling or perspective operations.

Transforms are assigned and manipulated by numerous
functions. The most basic include:
\begin{verbatim}
        TRSF_PTR copyTrsfXyz (tr, t1)
          TRSF_PTR tr, t1;

        TRSF_PTR copyTrsfNoa (tr, t1)
          TRSF_PTR tr, t1;

        TRSF_PTR multTrsf (tr, t1, t2)
          TRSF_PTR tr, t1, t2;

        TRSF_PTR multRiTrsf (tr, t1, t2)
          TRSF_PTR tr, t1, t2;

        TRSF_PTR multLiTrsf (tr, t1, t2)
          TRSF_PTR tr, t1, t2;

        TRSF_PTR invertTrsf (tr, t1)
          TRSF_PTR tr, t1;

        TRSF_PTR identTrsf (tr)
          TRSF_PTR tr;
\end{verbatim}
The functions
{\tt copyTrsfXyz()}
%\index {\tt copyTrsfXyz()}{}
and
{\tt copyTrsfNoa()}
%\index {\tt copyTrsfNoa()}{}
perform a selective copy of the translational (resp.\ rotational) part,
leaving untouched the rotational (resp.\ translational) part.
The function
{\tt multTrsf()}{}
%\index {\tt multTrsf()}{}
multiplies {\tt t1} by {\tt t2} and leaves the result
in {\tt tr}.
{\tt multRiTrsf()}{}
%\index {\tt multRiTrsf()}{}
multiplies {\tt t1} by the inverse of {\tt t2} and leaves the result
in {\tt tr}.
The function
{\tt multLiTrsf()}{}
%\index {\tt multLiTrsf()}{}
multiplies the inverse of {\tt t1} by {\tt t2} and leaves the result
in {\tt tr}.
The function
{\tt invertTrsf()}
%\index {\tt invertTrsf()}{}
takes the inverse of {\tt t1} and leaves it in {\tt tr}.
The function
{\tt identTrsf()}
%\index {\tt identTrsf()}{}
converts {\tt tr} to an identity transform.

Again, all of these routines return a pointer to their result argument.

The following additional functions are available to selectively set various 
components of a transformation:
\begin{verbatim}
        TRSF_PTR xyzToTrsf (tr, px, py, pz)
          TRSF_PTR tr;
          float px, py, pz;

        TRSF_PTR vaoToTrsf (tr, ax, ay, az, ox, oy, oz)
          TRSF_PTR tr;
          float ax, ay, az, ox, oy, oz;

        TRSF_PTR rotToTrsf (tr, k, h)
          TRSF_PTR tr;
          VECT_PTR k;
          float h;

        TRSF_PTR eulToTrsf (tr, phi, the, psi)
          TRSF_PTR tr;
          float phi, the, psi;

        TRSF_PTR rpyToTrsf (tr, roll, pitch, yaw)
          TRSF_PTR tr;
          float roll, pitch, yaw; 
\end{verbatim}
All these functions use a transformation pointer as left argument;
as usual, this pointer is also returned by the function.

The function
{\tt xyzToTrsf()}
%\index {\tt xyzToTrsf()}{}
sets the terms of the ${\bf p}$ vector of {\tt tr} and leaves
the rotational part untouched.

The function
{\tt vaoToTrsf()}
%\index {\tt vaoToTrsf()}{}
sets the ${\bf n}$, ${\bf o}$, and ${\bf a}$ vectors of {\tt tr}.
Since the vectors ${\bf n}$, ${\bf o}$ and ${\bf a}$ are orthogonal,
{\tt vaoToTrsf()}
%\index {\tt vaoToTrsf()}{}
takes only the components of ${\bf o}$ and ${\bf a}$ and builds the
vector ${\bf n}$ itself.
The vectors whose components are input as arguments do not need to
be orthogonal, since
the rotational part of the transform is built as follows:
take the
user's supplied ${\bf a}$ vector, normalize it and use it as the final 
${\bf a}$ vector;
take
the user's supplied ${\bf o}$ vector (which need not be orthogonal to {\bf a})
and build a (possibly non-unit) vector ${\bf n}$ that is orthogonal to
both ${\bf
o}$ and ${\bf a}$;
reconstruct ${\bf o}$ to be orthogonal with ${\bf n}$ and ${\bf a}$,
normalize it, and finally, compute
${\bf n}$ from the cross product of ${\bf o}$ and ${\bf a}$\footnote{Check the reference manual for the corresponding equations.}.

The function
{\tt rotToTrsf()}
%\index {\tt rotToTrsf()}{}
sets the rotational part of {\tt tr} to a rotation 
of {\tt h} degrees about
a (possibly non-unit) vector {\tt k}.

The function
{\tt eulToTrsf()}
%\index {\tt eulToTrsf()}{}
sets the rotational part of {\tt tr} to a rotation expressed
with the three Euler angles (in degrees).

Finally, the function
{\tt rpyToTrsf()}
%\index {\tt rpyToTrsf()}{}
sets the rotational part of {\tt tr} to a rotation expressed
with the three roll-pitch-yaw angles (in degrees).

The next set of functions are exactly
analogous to the previous ones, except
that the transform argument is
{\it post-multiplied\/}
by another transform
{\it defined by\/}
the other parameters of the function.
As usual, a pointer to the result transform is returned as the value of the
function.
\begin{verbatim}
        TRSF_PTR multTrsfXyz (tr, px, py, pz)
          TRSF_PTR tr;
          float px, py, pz;

        TRSF_PTR multTrsfVao (tr, ax, ay, az, ox, oy, oz)
          TRSF_PTR tr;
          float ax, ay, az, ox, oy, oz;

        TRSF_PTR multTrsfRot (tr, k, h)
          TRSF_PTR tr;
          VECT_PTR k;
          float h;

        TRSF_PTR multTrsfEul (tr, phi, the, psi)
          TRSF_PTR tr;
          float phi, the, psi;

        TRSF_PTR multTrsfRpy (tr, roll, pitch, yaw)
          TRSF_PTR tr;
          float roll, pitch, yaw;
\end{verbatim}

%\index {\tt multTrsfXyz()}{}
%\index {\tt multTrsfVao()}{}
%\index {\tt multTrsfRot()}{}
%\index {\tt multTrsfEul()}{}
%\index {\tt multTrsfRpy()}{}
It may sometimes be desirable to convert the rotational component of a
{\tt TRSF} data type back into Euler angles,
roll-pitch-yaw angles, or rotation angle/axis parameters.
The functions to do this are:
\begin{verbatim}
        trsfToEul (phi, the, psi, t1)
          float *phi, *the, *psi;
          TRSF_PTR t1;

        trsfToRpy (roll, pitch, yaw, t1)
          float *roll, *pitch, *yaw;
          TRSF_PTR t1;

        trsfToRot (k, h, t1)
          VECT_PTR k;
          float *k;
          TRSF_PTR t1;
\end{verbatim}
%\index {\tt trsfToEul()}{}
%\index {\tt trsfToRpy()}{}
%\index {\tt trsfToRot()}{}

The Euler or roll-pitch-yaw angle representations for a rotation are
not necessarily unique. The rotation angle/axis representation is unique
since the returned value of {\tt h} is defined to lie in the range
$0 \le h < 180^\circ$.
Note also that scalar values are returned
through {\it pointers}, in a manner 
analogous to the UNIX routine {\tt scanf()}.

\section{Allocating Transformations}

Structures of the {\tt TRSF} data type described in the previous section 
may be defined or allocated in any of the ways normally available
for creating C objects (static or automatic declaration, use of
{\tt malloc()}, etc.). However, if one wishes to create a transform
which is shared between the planning and 
control levels, then it must not be a static or automatic variable, nor
can it be {\tt malloc()}ed; instead, it must be allocated
via a set of special RCCL primitives. Transforms allocated in this
way can be bound to real-time functions, and can also be
used to form position equations as described later.

The basic primitives for creating and deleting transforms are
\begin{verbatim}
        TRSF_PTR allocTrans (name, pool)
          char *name;
          int pool;

        freeTrans (t)
          TRSF_PTR t;
\end{verbatim}
%\index {\tt allocTrans()}{}
%\index {\tt freeTrans()}{}
{\tt allocTrans()} allocates a transformation, gives it the indicated
name, sets its value to the identity,
and returns a pointer to it. The variable {\tt pool} allows
the programmer to specify which memory pool the transform should be
allocated from.
Under the current implementation, the pool number
corresponds to a CPU number; i.e., specifying pool number 2 would
mean that the transform is allocated from memory on CPU 2.
Obviously, this is relevant only on multi-CPU versions of RCCL/RCI.
Typically, the pool argument is left undefined ({\tt UNDEF})
and the system chooses the allocation pool. It also is not necessary to give
the transform a name; if no name is desired, then that argument may be
specified as {\tt NULL}. Names are useful sometimes when one wishes to obtain
a pointer to the transform from the control level.

The function {\tt freeTrans()} returns a transform to the pool
from which it was allocated.

If a programmer wishes to make a compile-time
declaration of a transform, as in
\begin{verbatim}
        function()
        {
                TRSF base;

                ...
                ...
        }
\end{verbatim}
or the declaration
\begin{verbatim}
        static TRSF base;
\end{verbatim}
then the following should be noted:
\bigskip
\begin{list}{}{}
\item [$\bullet$]
First, transforms created this way
{\it cannot} be accessed by the control level. They
{\bf must not be used}
in a position
data structure (created by the function 
{\tt makePosition()}, 
%\index {\tt makePosition()}{}
described later).
\bigskip
\item [$\bullet$]
Structure data types are passed by reference,
so literally declared transformations
need to be prefixed with the {\tt \&} (address) operator
when passed to a function.
\bigskip
\item [$\bullet$]
It is up to the programmer to initialize transforms declared in this
way (for instance, by using {\tt identTrsf()}).
\end{list}

In addition to 
{\tt allocTrans()}, 
%\index {\tt allocTrans()}{}
there is a group of other functions which
allocate a new transform and at the same time
initialize it to desired values:
\begin{verbatim}
        TRSF_PTR allocTransXyz (name, pool, px, py, pz)
          char *name;
          int pool;
          float px, py, pz;

        TRSF_PTR allocTransRot (name, pool, px, py, pz, k, h)
          char *name;
          int pool;
          float px, py, pz, h;
          VECT_PTR k;

        TRSF_PTR allocTransPao (name, pool, px,py,pz,ax,ay,az,ox,oy,oz)
          char *name;
          int pool;
          float px, py, pz, ax, ay, az, ox, oy, oz;

        TRSF_PTR allocTransEul (name, pool, px, py, pz, phi, the, psi)
          char *name;
          int pool;
          float px, py, pz, phi, the, psi;

        TRSF_PTR allocTransRpy (name, pool, px, py, pz, roll, pitch, yaw)
          char *name;
          int pool;
          float px, py, pz, roll, pitch, yaw;
\end{verbatim}
%\index {\tt allocTransXyz()}{}
%\index {\tt allocTransRot()}{}
%\index {\tt allocTransPao()}{}
%\index {\tt allocTransEul()}{}
%\index {\tt allocTransRpy()}{}
All of these functions return a pointer to the created transform.
The first two arguments are identical to the first two arguments
of {\tt allocTrans()}.
The next three arguments specify the translational component, the
${\bf p}$ vector.
The remaining arguments, if any, define the rotational component, and
are the same as those
for the corresponding xxx{\tt ToTrsf()} routines
(except for
{\tt allocTransPao()}, which maps onto {\tt vaoToTrsf()} instead of {\tt
paoToTrsf()}). 

For example, the code fragment
\begin{verbatim}
        TRSF_PTR t1, t2, t3;    /* declare transform pointers */

        ...

        t1 = xyzToTrsf (eulToTrsf (allocTrans ("T1", UNDEF), 
                                   10.0, 20.0, 30.0), 
                        1.0, 2.0, 3.0);

        t2 = allocTransEul ("T2", UNDEF, 1.0, 2.0, 3.0, 10.0, 20.0, 30.0);

        t3 = allocTransXyz ("T3", UNDEF, 1.0, 2.0, 3.0);
        eulToTrsf (t3, 10.0, 20.0, 30.0);
\end{verbatim}
produces three identical transforms.

Finally, there are functions for printing the value of a transform:
\begin{verbatim}
        printTrsf (fmt, tr);
        char *fmt;
        TRSF_PTR tr;

        fprintTrsf (fp, fmt, tr);
        FILE *fp;
        char *fmt;
        TRSF_PTR tr;

        rciPrintTrsf (fmt, tr);
        char *fmt;
        TRSF_PTR tr;
\end{verbatim}
%\index {\tt printTrsf()}{}
%\index {\tt fprintTrsf()}{}
%\index {\tt rciPrintTrsf()}{}
All of these accept a pointer to a transform and a
format string describing how the transform should be
printed. Typical format strings are {\tt "\%m\\n"}, which
prints the transform in matrix form followed by a newline,
{\tt "\%r\\n\%p\\n"}, which prints the transform in terms of
the roll-pitch-yaw angles and the translational component followed
by an newline, and
{\tt "\%q\\n\%p\\n"}, which prints the transform in terms of
a quaternion and the translational component (again, followed by
a newline). Preceding
one of the format characters with a {\tt 'L'} causes one of the
labels 
\begin{verbatim}
        QTRN
         RPY
         XYZ
         EUL
\end{verbatim}
to be printed in front of the quaternion, roll-pitch-yaw, translational,
or euler angle representation. Non-format characters which appear in
the format string are printed literally (hence the new lines in the above
examples).

For example, the code fragment
\begin{verbatim}
        TRSF_PTR t1, t2, t3;

        t1 = allocTransEul ("T1", UNDEF, 10., 20., 30., 11., 12., 13.);
        printTrsf ("T1 %m\n", t1);

        t2 = allocTrans ("T2", UNDEF);
        rotToTrsf(t2, yunit, 90.);
        multTrsfXyz(t2, 10., 20., 30.);
        putchar ('\n');
        printTrsf ("T2 %Lr\n   %Lp\n", t2);

        t3 = allocTrans ("T3", UNDEF);
        putchar ('\n');
        printTrsf ("T3 %m\n", multTrsf (t3, t1, t2));

        putchar ('\n');
        printTrsf ("T3 %q\n   %p\n", t3);
\end{verbatim}
will produce the output
\begin{verbatim}
        T1  0.89264 -0.40191  0.20409    10.00
            0.40267  0.91448  0.03967    20.00
           -0.20258  0.04677  0.97815    30.00
        
        T2     RPY:    0.000   90.000   -0.000
               XYZ:    30.00    20.00   -10.00
        
        T3 -0.20409 -0.40191  0.89264    26.70
           -0.03967  0.91448  0.40267    49.97
           -0.97815  0.04677 -0.20258    15.08
        
        T3  0.61396 -0.14492  0.76177  0.14750
              26.70    49.97    15.08
\end{verbatim}

Much more detailed information on the format string syntax
can be found in the manual page for {\tt printTrsf()}.

The functions {\tt fprintTrsf()} and {\tt rciPrintTrsf()}
are identical to {\tt printTrsf()} except that the first allows a
file stream to specified for output and the latter can be
called from the control level (with the output going to the
same place as for {\tt rciPrintf()}).

\section{Differential Motions and Forces}

{\it NOTE:
The background to this material is described in [Paul 1981]; this section can
probably be omitted on first reading.}

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:
\begin{verbatim}
        typedef struct {
                VECT t, r;
        } DIFF, *DIFF_PTR;

        typedef struct {
                VECT f, m;
        } FORCE, *FORCE_PTR;
\end{verbatim}
Since velocity is simply a differential displacement divided 
by a differential time change, the {\tt DIFF} data type
can also be used to represent velocities.

The functions associated with these data types are:
\begin{verbatim}
        TRSF_PTR diffToTrsf (tr, d1)
          TRSF_PTR tr;
          DIFF_PTR d1;

        DIFF_PTR trsfToDiff (dr, t1)
          DIFF_PTR dr;
          TRSF_PTR t1;

        DIFF_PTR transDiff (dr, d1, t)
          DIFF_PTR dr, d1;
          TRSF_PTR t;

        FORCE_PTR transForce (fr, f1, t)
          FORCE_PTR fr, f1;
          TRSF_PTR t;
\end{verbatim}
%\index {\tt copyDiff()}{}
%\index {\tt diffToTrsf()}{}
%\index {\tt trsfToDiff()}{}
%\index {\tt transDiff()}{}
%\index {\tt copyForce()}{}
%\index {\tt transForce()}{}

The function
{\tt diffToTrsf()}
%\index {\tt diffToTrsf()}{}
converts a {\tt DIFF} type into the corresponding
transform; the rotational displacement described by {\tt DIFF} is
assumed to be small enough so the conversion is reasonably well behaved.
The function
{\tt trsfToDiff()}
converts a transform into
a {\tt DIFF} type (again, the rotational displacement
is assumed to be small).

The function
{\tt transDiff()}
transforms a differential motion expressed with respect
to one frame into the same differential motion expressed with respect to
another frame.
Suppose the original frame P1 is described by
a homogeneous transform {\bf P1} and the destination frame P2
is described by another transform {\bf P2}.
The transform argument {\tt t} is then defined to link these so that
\[
{\bf P2} \;=\; {\bf P1} \; {\bf T}.
\]
The input differential motion {\tt d1} is described with respect to
P1 and the output {\tt dr} is described with respect to P2.

The function {\tt transForce()}
%\index {\tt transForce()}{}
perform the analogous transformation on {\tt FORCE} types.

To print out the values of {\tt DIFF} or {\tt FORCE} structures, the
RCI utility routine {\tt printVf()} can be used. This is a general
purpose function used for printing a vector (or array) of floats.
It is declared as follows:
\begin{verbatim}
        printVf (fmt, v, n)
          char *fmt;
          float *v;
          int n; 
\end{verbatim}
where {\tt fmt} is a format string, {\tt v} is a pointer to the
vector, and {\tt n} is the number of elements in the vector.
The format string accepts the usual constructs used by {\tt printf()}
for outputting floating point numbers (such as {\tt "\%f"} and {\tt "\%g"}),
except that the number of output fields is set equal to the number
of elements in the vector. The {\tt DIFF} and {\tt FORCE}
structures can usually be
printed by this routine,
if the number of elements is set to 6\footnote{It is possible (though unlikely)
that the internal representation of these data types 
on some machines will not be equivalent to an array of floats, in which case
this will not work. Casting the address
of the structure to a float pointer can also fail, 
but again this is improbable
on most systems likely to run RCCL.}.

For example, the following sequence of program statements
\begin{verbatim}
        DIFF  Dp1, Dp2;
        FORCE Fp1, Fp2;
        TRSF *t;

        t = allocTransPao ("T", UNDEF, 10.,5.,0.,1.,0.,0.,0.,0.,1.);

        printTrsf ("%m\n", t);

        Dp2.t.x =   1.0;
        Dp2.t.y =   0.0;
        Dp2.t.z =   0.5;
        Dp2.r.x =   0.0;
        Dp2.r.y =   0.1;
        Dp2.r.z =   0.0;
        printVf ("DP2: %6.2f\n", (float*)&Dp2, 6);
        printVf ("DP1: %6.2f\n", (float*)transDiff (&Dp1, &Dp2, t), 6);

        Fp2.f.x =  10.0;
        Fp2.f.y =   0.0;
        Fp2.f.z =   0.0;
        Fp2.m.x =   0.0;
        Fp2.m.y = 100.0;
        Fp2.m.z =   0.0;
        printVf ("FP2: %6.2f\n", (float*)&Fp2, 6);
        printVf ("FP1: %6.2f\n", (float*)transForce(&Fp1, &Fp2, t), 6);
\end{verbatim}
will yield the following output:
\begin{verbatim}
         0.00000  0.00000  1.00000    10.00
         1.00000  0.00000  0.00000     5.00
         0.00000  1.00000  0.00000     0.00
        DP2:   1.00   0.00   0.50   0.00   0.10   0.00 
        DP1:   0.00  -0.50   1.00   0.10   0.00   0.00 
        FP2:  10.00   0.00   0.00   0.00 100.00   0.00 
        FP1:   0.00   0.00  10.00 100.00  50.00   0.00 
\end{verbatim}

Much more detailed information on {\tt printVf()} can be found
in the manual pages.

For printing values to a file, or doing diagnostic printing from
the control level, 
the functions {\tt fprintVf()} and {\tt rciPrintVf()}
can be used; these are identical to 
{\tt printVf()} except that the first allows a
file stream to specified for output, and the latter can be
called from the control level (with the output going to the
same place as for {\tt rciPrintf()}).

\section{Displacements}
%\index {DSPL}{}
%\index {displacements}{}

Any displacement in Cartesian coordinates can be represented
as a translation followed by a single rotation about some
axis. RCCL provides a data type for representing displacements
this way:
\begin{verbatim}
        typedef struct {
                VECT p;
                float a;
                VECT u;
        } DSPL, *DSPL_PTR;
\end{verbatim}
The {\tt p} field describes the translation, the {\tt a} field describes the
rotation angle (in radians), and the {\tt u} field is a unit vector parallel
to the axis of rotation. The rotational representation is potentially
ambiguous: if the {\tt a} and {\tt u} fields describe
$a$ and ${\bf u}$, then $-a$ and $-{\bf u}$ represent the same thing.
To resolve this, the range of $a$ is restricted to $0 \le a \le \pi$.
(The rotation representation is the same as that used
by {\tt rotToTrsf()}.)

The {\tt DSPL} data type contains
all the information described by a {\tt TRSF}
data type, and vice-versa. It is sometimes the preferred
representation
because the associated parameters 
are easy to scale:
all one needs to do is multiply the translation vector {\tt p} and
the rotation angle {\tt a} by some factor.

The following functions are available for manipulating
{\tt DSPL} data types:
\begin{verbatim}
        DSPL_PTR trsfToDspl (dr, t1)
          DSPL_PTR dr;
          TRSF_PTR t1;

        TRSF_PTR dsplToTrsf (tr, d1)
          TRSF_PTR tr;
          DSPL_PTR d1;

        DSPL_PTR scaleDspl (dr, s, d1)
          DSPL_PTR dr;
          float s;
          DSPL_PTR d1;

        TRSF_PTR extrapTrsfByDspl (tr, s, d1)
          TRSF_PTR tr;
          float s;
          DSPL_PTR d1;

        DSPL_PTR unitDspl (dr, d1)
          DSPL_PTR dr, d1;
\end{verbatim}
The functions {\tt trsfToDspl()} and {\tt dsplToTrsf()} convert
between {\tt TRSF} and {\tt DSPL} representations.
The function {\tt scaleDspl()} scales the values of {\tt d1} by 
{\tt s} and puts the result in {\tt dr}. The function
{\tt extrapTrsfByDspl()} is a combination of {\tt scaleDspl()}
and {\tt dsplToTrsf()}: it scales the values of {\tt d1}
by {\tt s} and then converts the result into the transform {\tt tr}.
The function {\tt unitDspl()} 
normalizes the values of the {\tt a} and
{\tt u} fields of {\tt d1} and places the result in {\tt dr}.

Because its parameters scale easily, 
{\tt DSPL} data types are useful for representing velocities.
A general Cartesian velocity described by the
vectors ${\bf v}$ and ${\bf \omega}$ can be represented with a 
{\tt DSPL} data type by setting the {\tt p} field
to ${\bf v}$, the {\tt a} field to
$|{\bf \omega}|$, and the {\tt u} field to ${\bf \omega}/|{\bf \omega}|$.
The function {\tt scaleDspl()} can then be used
to determine the net displacement achieved by this velocity 
over some time interval $t$. The equivalent transform representation
of the same displacement
can be computed with 
{\tt extrapTrsfByDspl()}:
\begin{verbatim}
        DSPL vel;
        TRSF delta;
        float time;

        ... variables are set ...

        extrapTrsfByDspl (&delta, time, &vel);  
\end{verbatim}

\section{Quaternions}
%\index {quaternions}{}

RCCL has another data type for describing quaternions
which are useful for representing rotations.
\begin{verbatim}
        typedef struct {
                float c;
                VECT v;
        } QTRN, *QTRN_PTR;
\end{verbatim}
A rotation can be represented as a single rotation $\theta$ about a unit
vector ${\bf v}$. A quaternion is a four tuple; when normalized
to have unit length, it represents a rotation such that the first
value is equal to $\cos (\theta/2)$ and the last three values form a
vector equal to $\sin (\theta/2) {\bf v}$. Since multiplication of
quaternions is equivalent to successive rotation operations and
quaternion multiplication is faster that homogeneous transform
multiplication,
the use of quaternions can sometimes be desirable.

Quaternions used by RCCL are assumed to be unit quaternions.
They are assigned and manipulated by the following functions:
\begin{verbatim}
        QTRN_PTR multQtrn (qr, q1, q2)
          QTRN_PTR qr, q1, q2;

        QTRN_PTR multRiQtrn (qr, q1, q2)
          QTRN_PTR qr, q1, q2;

        QTRN_PTR multLiQtrn (qr, q1, q2)
          QTRN_PTR qr, q1, q2;

        QTRN_PTR invertQtrn (qr, q1)
          QTRN_PTR qr, q1;

        float normQtrn (qr)
          QTRN_PTR qr;

        QTRN_PTR unitQtrn (qr)
          QTRN_PTR qr;
\end{verbatim}
The function
{\tt multQtrn()}{}
%\index {\tt multQtrn()}{}
multiplies {\tt q1} by {\tt q2} and leaves the result
in {\tt qr}.
{\tt multRiQtrn()}{}
%\index {\tt multRiQtrn()}{}
multiplies {\tt q1} by the inverse of {\tt q2} and leaves the result
in {\tt qr}.
The function
{\tt multLiQtrn()}{}
%\index {\tt multLiQtrn()}{}
multiplies the inverse of {\tt q1} by {\tt q2} and leaves the result
in {\tt qr}.
The function
{\tt invertQtrn()}
%\index {\tt invertQtrn()}{}
takes the inverse of {\tt q1} and leaves it in {\tt qr}.
The function
{\tt normQtrn()}
%\index {\tt normQtrn()}{}
returns the magnitude of the quaternion (which should normally be 1).
The function
{\tt unitQtrn()}
%\index {\tt unitQtrn()}{}
normalizes the quaternion {\tt qr}.

The following additional functions are used to convert between
quaternions and other data types:
\begin{verbatim}
        TRSF_PTR qtrnToTrsf (tr, q1)
          TRSF_PTR tr;
          QTRN_PTR q1;

        QTRN_PTR trsfToQtrn (qr, t1)
          QTRN_PTR qr;
          TRSF_PTR t1;

        VECT_PTR qtrnToVect (vr, q1)
          VECT_PTR vr;
          QTRN_PTR q1;
\end{verbatim}
The function
{\tt qtrnToTrsf()} sets the rotational component of the transform {\tt tr}
%\index {\tt qtrnToTrsf()}{}
to the rotation defined by the quaternion {\tt q1}.
The function
{\tt trsfToQtrn()} sets the quaternion {\tt qr}
%\index {\tt trsfToQtrn()}{}
to the rotation defined by the rotational 
component of the transform {\tt tr}.
The function
{\tt qtrnToVect()} sets the vector {\tt vr}
%\index {\tt qtrnToVect()}{}
to the rotational velocity indicated by the quaternion {\tt q1} 
(where
the magnitude of the rotation is assumed to be small).

The printing of quaternion values can be done using {\tt printVf()},
with the number of elements in the ``vector'' being specified as 4.

\section{Generic Vector Routines}

There are several routines available for doing simple operations
on arbitrarily sized vectors of floats ({\it i.e.}, arrays of
floats). These routines are imported directly from RCI, and this
description also appears in the RCI user's guide.

To add, element by element, two equal sized vectors of floats, the routine
\begin{verbatim}
        addVf (vr, v1, v2, size) 
\end{verbatim}
can be used, where {\tt vr} is the result vector, {\tt v1} and {\tt v2} are
the input vectors, and {\tt size} is the number of elements in the vector.
The {\tt f} at the end of the routine name indicates that the procedure
handles vectors of floats. An analogous routine, {\tt addVd}, exists
for vectors of doubles.

Similar routines do other arithmetic operations:
\begin{verbatim}
        subtractVf (vr, v1, v2, size)
        scaleVf (vr, a, v1, size)
        combineVf (vr, a, v1, b, v2, size)
        dotVf (v1, v2, size);
        normVf (v1, size); 
\end{verbatim}
{\tt subtractVf()} subtracts {\tt v1} from {\tt v2}, {\tt scaleVf()}
multiplies {\tt v1} by the scalar {\tt a}, {\tt combineVf()} forms the linear
combination
\[
{\bf v}_r =  a \; {\bf v}_1 + b \; {\bf v}_2,
\]
{\tt dotVf()} returns a float equal to the dot product of {\tt v1} and
{\tt v2}, and {\tt normVf()} returns a float equal to the 2-norm of 
{\tt v1}.

To copy vectors, we can use the routine
\begin{verbatim}
        copyVf (vr, v1, size); 
\end{verbatim}
and to zero a vector, we can use
\begin{verbatim}
        zeroVf (vr, size); 
\end{verbatim}

Printing a vector can be done using {\tt printVf()}, {\tt fprintVf()},
or {\tt rciPrintVf()}, described above.
Reading a vector in can be accomplished using either of
\begin{verbatim}
        scanVf (vr, size);
        fscanVf (fp, vr, size); 
\end{verbatim}
which input a vector of {\tt size} elements either from {\tt stdin} or
the stream {\tt fp}, respectively.

\section{Joint Coordinates}

The data type {\tt JNTS} is used to describe sets of joint coordinates
for a particular manipulator. The structure is presently defined
as 
\begin{verbatim}
        typedef struct {
                float v[NUM_JNTS];
                char type[NUM_JNTS];
                int num;
        } JNTS, *JNTS_PTR; 
\end{verbatim}
where {\tt NUM_JNTS} is, at the time of this writing, 6.
The field {\tt v} contains the value of each joint coordinate, with
the field {\tt type} describing its type
({\tt 'R'} for revolute joints and {\tt 'P'} for prismatic ones).
The field {\tt num} gives the total number of joints, where it
is assumed that {\tt num} is less than or equal to {\tt NUM_JNTS}.
The units are normally assumed to be radians for rotational
joints and millimeters for translational joints.

A set of basic routines for manipulating {\tt JNTS} types is:
\begin{verbatim}
        JNTS_PTR addJnts (jr, j1, j2)
          JNTS_PTR jr, j1, j2;

        JNTS_PTR subtractJnts (jr, j1, j2)
          JNTS_PTR jr, j1, j2;

        JNTS_PTR scaleJnts (jr, a, j1)
          JNTS_PTR jr, j1;
          float a;

        JNTS_PTR combineJnts (jr, a, j1, b, j2)
          JNTS_PTR jr, j1, j2;
          float a, b;

        JNTS_PTR zeroJnts (jr)
          JNTS_PTR jr;
\end{verbatim}
The function
{\tt addJnts()}
%\index {\tt addJnts()}{}
adds the joint values {\tt j1} to {\tt j2} and places the result in {\tt jr}.
The function
{\tt subtractJnts()}
%\index {\tt subtractJnts()}{}
subtracts the joint values {\tt j2} from {\tt j1} and places the 
result in {\tt jr}.
The function
{\tt scaleJnts()}
%\index {\tt scaleJnts()}{}
multiples the joint values {\tt j1} by {\tt a} and places the result in 
result in {\tt jr}.
The function
{\tt combineJnts()}
%\index {\tt combineJnts()}{}
computes 
\[
\hbox{\tt jr} = \hbox{\tt a} \; \hbox{\tt j1} + \hbox{\tt b} \; \hbox{\tt j2}
\]
and places the result in {\tt jr}.
{\tt zeroJnts()} zeros the values of {\tt jr}.

The above routines do not change either the {\tt type}
or {\tt num} field of their result arguments (although
this may change). Since the contents of
{\tt type} and {\tt num} fields
are robot specific, there must be a way to initialize them for
a particular manipulator. Two routines are available to do this:
\begin{verbatim}
        initJntsByName (jr, name)
          JNTS_PTR jr;
          char *name;

        initJntsByManip (jr, mnp)
          JNTS_PTR jr;
          MANIP_PTR mnp; 
\end{verbatim}
{\tt initJntsByName()} instantiates {\tt num} and {\tt type} for
the manipulator with the indicated name. This routine can be called only from
the planning level. {\tt initJntsByManip()} instantiates {\tt num} 
and {\tt type} for a robot described by a {\tt MANIP} structure
({\tt MANIP} is the 
RCCL data type used to reference and control a robot from within 
a program, and will be introduced in section \ref{create}).

The printing of joint values can be done using {\tt printVf()},
with the number of elements in the ``vector'' being specified by
the {\tt num} field. If one wants to first convert the angle values
to degrees, the following piece of code will do the job,
assuming that all joints are revolute:
\begin{verbatim}
        JNTS jnts;      /* initialized somewhere */

        { JNTS deg;
          scaleJnts (&deg, RADTODEG, &jnts);
          printVf ("%8.3f\n", deg.v, jnts.num);
        } 
\end{verbatim}
Notice that in the call the {\tt printVf()} the joint angles are referenced
by the {\tt v} field of the structure, since this routine expects
an array of floats.

A more complicated version that considers whether or not the joints
are prismatic is
\begin{verbatim}
        JNTS jnts;      /* initialized somewhere */
        int j;

        for (j=0; j<jnts.num; j++)
         { if (jnts.type[j] == 'r')
            { printf ("%8.3f ", RADTODEG*jnts.v[j]);
            }
           else
            { printf ("%8.3f ", jnts.v[j]);
            }
         } 
        putchar ('\n'); 
\end{verbatim}

\begin{quote}
{\bf Caveat:} The addition of the {\tt type} and {\tt num}
fields to 
the {\tt JNTS} data structure is new, and not all RCCL
code has been updated to use them. Prior to their introduction,
the number of joints was assumed to be described by the constant
{\tt NUM_JNTS}.
\end{quote}

\newpage
\done
