\input rccluginit.latex

\chapter{Moving to Variable Targets}

\section{Transform Bindings}\label{bindings}

The transforms which make up a position equation are objects
which describe positional relationships in the environment.
Since these relationships may change, it should be possible to
provide ways for changing the values of the associated transforms.

By default, an RCCL transform allocated using one of the {\tt allocTrans()}
routines is assumed to be
{\it constant}. That means that whenever that transform is used 
in a motion request (as part of a position equation)
the trajectory generator is free
to assume that the value of that transform will not change for the
duration of the motion. When {\tt move()} is called,
a private copy of the transform's
current value is made and sent to the trajectory
generator along with the motion request packet.
If the transform's value is changed {\it after} the call
to {\tt move()}, the change
will affect only subsequent motion requests.

While this is a reasonable paradigm,
one may not always want a transform to be assumed constant.
It may be desirable for the trajectory generator to
consider a particular transform to be {\it variable}
and constantly read its value and
adjust the target position accordingly. In other words, a motion
to a target that contains a variable transform will {\it track} any
changes that variable transform makes. The value of a transform can
be changed by either the planning task or by functions 
being executed through the trajectory generator. 

The primitive to declare a transform variable is
\begin{verbatim}
        setTransVarb (tr)
          TRSF_PTR tr;
\end{verbatim}
After this call is made, subsequent motions which reference the
transform will track any changes to it. These changes
will be tracked literally, without any of the acceleration limiting that is
normally applied between adjacent motions. One should therefore
be careful to ensure that changes to such transforms are smooth and clean.
If a variable transform contained in a target position
changes suddenly by a large amount,
the robot will
try to ``leap'' across the work space\footnote{Which is a bad thing, because
if the robot is large, it might succeed.}. One reason that variable
transforms should usually be changed from the control level is
to ensure that the changes to it are made at a constant rate.

A transform may be set back to being a constant transform with the
primitive
\begin{verbatim}
        setTransConst (tr)
          TRSF_PTR tr;
\end{verbatim}

One special way of making a transform variable is to bind it to
to a function executed at the control level:
\begin{verbatim}
        transEval (tr, fxn, arg) 
          TRSF_PTR tr;
          int (*fxn)();
          int arg;
\end{verbatim}
This instructs the trajectory generator to call the
function {\tt fxn} once every control cycle to adjust the
value of {\tt tr}.
The function will be called
with the following arguments:
\begin{verbatim}
        fxn (tr, arg)
        TRSF_PTR tr;
        int arg;
        {
                ... modify the value of the transform ...
        }
\end{verbatim}
{\tt tr} points to the transform being updated (or actually a copy
of it; the real transform is updated atomically as soon as the updating
function returns), and {\tt arg} is the application-defined argument
specified by the third parameter to {\tt transEval()}.
The trajectory generator begins executing the transform function
immediately, and continues to do so until the binding is removed
(such as with a call to {\tt setTransVarb()} or {\tt setTransConst()}).

A transform may also be bound to a function that is executed only when
the transform is contained within the manipulator's current target position.
This is done with the call
\begin{verbatim}
        transMotionEval (tr, fxn, arg, mnp)
          TRANS_PTR tr;
          int (*fxn)();
          int arg;
          MANIP *mnp;
\end{verbatim}
The function {\tt fxn} will be called to update {\tt tr}
only when {\tt mnp} is executing a motion that contains
{\tt tr} as part of the target specification
(this is actually the way function binding in the
old RCCL system worked).
For this sort of function binding, the trajectory generator
calls {\tt fxn} with a pointer to the associated
{\tt MANIP} structure as an additional argument:
\begin{verbatim}
        fxn (tr, arg, mnp)
        TRSF_PTR tr;
        int arg;
        MANIP *mnp;
        {
                ... modify the value of the transform ...
        }
\end{verbatim}

An alternative way of setting the transform bindings discussed above
is to use the pair of functions
\begin{verbatim}
        setTransBinding (tr, binding)
          TRSF_PTR tr;
          TRANS_BINDING *binding;

        getTransBinding (tr, binding)
          TRSF_PTR tr;
          TRANS_BINDING *binding; 
\end{verbatim}
{\tt setTransBinding()} sets the binding for a transform and
{\tt getTransBinding()} reads the current binding back. Binding
information is specified or read back through the data type
{\tt TRANS_BINDING}, which is defined as follows:
\begin{verbatim}
        typedef struct {
                int code;
                int (*fxn)();
                int arg;
                MANIP *mnp;
        } TRANS_BINDING;
\end{verbatim}
The fields {\tt fxn}, {\tt arg}, and {\tt mnp} contain the same information
that is provided by the arguments to the functions
{\tt transEval()} and {\tt transMotionEval()}.
{\tt code} describes the type of binding and is set to one the four values
\begin{verbatim}
        TRANS_CONST
        TRANS_VARB
        TRANS_EVAL
        TRANS_MOTION_EVAL
\end{verbatim}
depending on the binding.

Function bindings may be applied to any transform in a position
equation. When a manipulator is asked to move to such a time varying target,
it will track the changes as it moves toward the target (and continue to track
the changes if it is asked to stop there).

When setting up functionally defined positions, as with
all position equations, it is important to realize
that the effect of varying a particular transform depends heavily on
where it is located within the position equation: we have to consider
the coordinate frame in which the transform is itself defined.

\begin{figure}[tbhp]
\boxedfullpagefig{\psfig{figure=figs/many.idr}}%
{Graphs for four different time-varying positions.}%
\label{many}%
\end{figure}

As an illustration of this, consider the position equations defined
in figure \ref{many}. Each of these contains a manipulator ${\bf T6}$
transform, a fixed transform {\bf BASE} which maps to the manipulator base
frame from a ``world'' coordinate frame W, a fixed
{\bf TOOL} transform that maps into a tool frame T, and a single variable
transform (either ${\bf T}(t)$ or ${\bf R}(t)$). The manipulator transform
is indicated by ${\bf T6}(t)$ as a reminder that it is time varying
as well. ${\bf T}(t)$ performs pure translation, and ${\bf R}(t)$ performs
pure rotation.

The first two position equations create pure translational motions, the first
with respect 
to the {\it world} frame W and the second with respect to
the {\it tool} frame T. The difference is obtained by changing the 
direction sense of ${\bf T}(t)$.
(Pure translation always produces a pure translation,
no matter what coordinate frame it is induced in.)  The difference
between the two motions would simply be difference in the orientation
of the line along which the motion was taking place.

The difference is more severe for the last two position equations, which
depict two
rotations applied in two different frames. A rotation in one frame may result
in both a rotation and a translation when observed from another frame.
In position (c), the center of rotation is fixed with respect
to W (and will move with respect to T), while in position (d), the center
of rotation will be fixed with respect to T (and will move with respect to W).
Rotations in tool coordinates are always hard to visualize because the 
tool frame itself is moving.

\section{Program Example: ``zigzag''}\label{zigzag}

In this simple example, the robot is programmed to move back and forth
between two positions which are moving as though they
are attached to a conveyor belt.
The net result is a sort of ``zigzag'' motion (see figure \ref{zigzagfig}).

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/zigzag.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/zigzagCtrl.560.c}
{\it 
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot,
and lives at {\tt zigzag.560.c} and {\tt zigzagCtrl.560.c}
in {\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 is contained
in {\tt zigzag.260.c} and {\tt zigzagCtrl.260.c}. For details
on how to compile it, see section \ref{rcc}.

}
\medskip

\begin{figure}[tbh]
%\rect 4.25truein by 5truein%
\boxedsizedfig{4.25truein}{5truein}%
{\psfig{figure=figs/zigzag.idr}}%
{Motion created in the program ``zigzag'' by moving back and forth between
two moving target points.}%
\label{zigzagfig}%
\end{figure}


The target position motion is created by binding a
transform to a real-time
function. Since this function executes at the control level, it must be
define in a separate module and loaded specially using the
{\tt rcc} command. In this and all other examples,
control level load modules are given a name that ends with
{\tt "Ctrl"}. The required compilation sequence is discussed in section
\ref{rcc}.

The program creates two manipulator target positions, both of
which are attached to a fictitious conveyor belt (\cref{1}). 
These are described by
the following equations:
\[
{\bf Z} \;\; {\bf T6} \;\; {\bf E} \;=\; {\bf CONV} \;\; {\bf T1} 
\]
and
\[
{\bf Z} \;\; {\bf T6} \;\; {\bf E} \;=\; {\bf CONV} \;\; {\bf T2} 
\]
These positions are a little more complicated than those in previous
examples.
To begin with, they both use a transform {\bf Z} which maps to
the base of the manipulator from some more
convenient frame (in this case located at the bottom of the robot's
pedestal). This is useful sometimes
because the pedestal base may make a more reasonable reference frame
than the manipulator base (which is often located
in some obscure spot like the middle of the robot's shoulder).
The location of the conveyor belt itself, relative to the pedestal base,
is then given by {\bf CONV}. The transforms 
{\bf T1} and {\bf T2} represent the location of each target
with respect to the conveyor frame.

The program starts up and brings the robot to the usual starting
position (\cref{2}). It then moves the robot to the first target point,
stops it there, waits for completion, and then requests
Cartesian interpolated motions (\cref{3}). The conveyor is not
yet ``in motion''; this is done next by using
{\tt transEval()} to bind {\tt conv} to the function
{\tt convfn} (\cref{4}). The application argument is used (in this case) to
indicate the conveyor velocity, in millimeters per second.
As soon as the function binding takes place, 
{\tt convfn()} will begin to ``move'' {\bf CONV} in a positive
direction along its $x$ axis. The program then issues a series
of requests to move the robot back and forth between the two
target points attached to the conveyor, stopping at each point
for one second.
Since the points are separated along the $y$ axis of the conveyor,
perpendicular to the direction of motion, this results in a
``zigzag'' like motion. Half way through the move-stop sequence, the 
direction of the conveyor is reversed with another call to 
{\tt transEval()} (\cref{6}), which leaves the function binding 
unchanged, but negates the velocity argument. When this is finished,
the program moves the robot back to the starting point and terminates in the
usual way (\cref{7}).

As mentioned above, the function 
{\tt convfn()} is defined in a separate load module
since it will be executed at the control level. It is defined to receive two
arguments: a pointer to the transform it is to modify and the application
argument. The application argument, which specifies the conveyor velocity in
millimeters per second, is multiplied by the RCCL sample interval
(obtained with {\tt rcclGetInterval()}) to obtain a per-cycle
increment which is added to the $x$ axis of the transform (\cref{8}).

\section{Restrictions for Control Level Functions}\label{restrictions}

Before proceeding further, we should describe in detail the restrictions that
apply to functions being executed at the control level.

Control level application functions are executed by the trajectory generator,
within the context of a trajectory task running on either the 
main UNIX CPU, or possibly an auxiliary CPU (see figure \ref{tasks}).
Which task/CPU is used is determined either by the system, or by 
a manipulator which may be associated with the function: monitor functions
(which will be described later) are explicitly associated with a manipulator,
and transform motion functions are implicitly associated with a manipulator.
Each manipulator is assigned to a particular trajectory task on a particular
CPU, and appliction functions associated with it are executed by the same
task.

The control level runs {\it outside} of UNIX, and hence does
not have access to any of the UNIX system calls (such as {\tt write()}
or {\tt fork()}), or any routine (such as {\tt malloc()} or {\tt printf()})
which might itself perform a UNIX system call.
This rules out certain library functions
that perform I/O, allocate memory, control processes, etc. 
In general, the safest course is to not use 
any UNIX library routines,
except for the math routines in {\tt libm.a} and the string procedures
in {\tt libc.a}. {\tt sprintf()} also appears to be harmless.
Likewise, some routines in the RCCL/RCI library 
cannot be called from the control level. Routines which allocate
structures, or initialize and control tasks, are usually in this
category. Whether or not an RCCL/RCI routine can be called from the
control level will generally be indicated in its reference manual 
page under the heading {\bf Restrictions}.

RCI provides a few surrogate routines to do things such as
memory allocation and diagnostic printing; see section \ref{rcisupport}.

Additional restrictions apply to
functions which are executed at the control level on the main UNIX CPU.
When the control level is executed on the UNIX CPU, it
is run in kernel mode, at maximum IPL,
off of the interrupt stack. This rather
draconian way of doing things is necessary in order to get the
necessary system response (UNIX simply cannot handle it). The RCI
system takes care to ensure that exceptions are handled properly 
(and do not result in a system crash, the way exceptions at that
priority normally do). However, there are still a few ways to crash the
system. One is to have the control level code execute an infinite
loop. This will simply hang the machine. Fortunately, infinite
loops turn out to be quite rare. Another way is to create a stray pointer
reference that happens to land in some bad place in system memory
(which is not protected because we are running in kernel mode).
Again, instances of this appear to be rare. A final way to cause the system to
crash is to overflow the interrupt stack, which is generally a fatal
condition on most machines. This can happen if the control level routines nest
too deeply, or if they declare too much automatic data. The fix for 
this is to simply redeclare the offending variables to be static.

Control level functions executing on auxiliary CPUs cannot cause
system crashes.

\section{Ways of Modifying Transforms With Functions}

There are several ways that a function bound to a transform may
change its value. We will mention a few of these here, although
the discussion is by no means exhaustive.

\begin{list}{}{}
\item[]{\it Absolute Changes}. The transform's value is set directly
each control cycle. For instance, we might have quantities
{\tt px}, {\tt py}, and {\tt pz} which are read in from some device such as 
a joystick. These could be used to set the transform's {\bf p}
vector, as in
\begin{verbatim}
        fxnAbs (tr, arg)
        TRSF *tr;
        int arg;
        {
                float px, py, pz;

                ... values are read in from somewhere ...

                xyzToTrsf (tr, px, py, pz);
        }
\end{verbatim}
\medskip

\item[]{\it Relative Changes}. The transform's value is changed incrementally.
A common way of accomplishing this is to multiply the transform's existing
value by a perturbation transform. Assume that the
quantities 
{\tt dx}, {\tt dy}, and {\tt dz} are read in from some sensor device.
The following sample function uses these to apply a relative translational
increment to its transform:
\begin{verbatim}
        fxnRel (tr, arg)
        TRSF *tr;
        int arg;
        {
                TRSF offset;
                float dx, dy, dz;

                ... values are read in from somewhere ...

                xyzToTrsf (&offset, dx, dy, dz);
                multTrsf (tr, tr, &offset);
        }
\end{verbatim}
\item[]
The same thing could be accomplished by a call to {\tt multTrsfXyz()}
instead of two calls to {\tt xyzToTrsf()} and {\tt multTrsf()}.
The
difficulty with relative changes is that they can cause errors
in the transform structure. This problem is more severe
when modifying the rotational component, which can lose
orthogonality quite easily.
This situation can be improved somewhat by renormalizing
the transform regularly, such as with the routine {\tt unitTrsf()}.
Another option is to use a representation for the net rotation
that has fewer parameters (this is particularly easy if the rotation
is about a single axis), change the reduced {\bf that}, and then convert it
into a transform once every cycle. This will in fact be done in the
example below.
\medskip

\item[]{\it Time Dependent Changes}. The changes to a transform
may depend explicitly on time. For instance, suppose that
we desire to make a transform rotate slowly about the $z$ axis
with a rotational velocity $\omega$. A function that will do this
is
\begin{verbatim}
        rotZ (tr, omega)
        TRSF *tr;
        int omega;      /* omega, in deg/sec */
        {
                float angle;
                static float t0 = F_UNDEF;

                if (t0 == F_UNDEF)
                 { t0 = rcclSysTime();
                 }
                angle = omega*(rcclSysTime() - t0)/1000.0;
                rotToTrsf (tr, zunit, angle);
        }
\end{verbatim}
\item[]
It obtains $\omega$ through the application argument
and uses {\tt rcclSysTime()} to get the current system
time in milliseconds. Because absolute time is being used, it is
necessary to keep track of the initial time;
this is done using the static variable
{\tt t0}. An equivalent function that does the same thing with
less fuss, using incremental changes, is
\begin{verbatim}
        rotZrel (tr, omega)
        TRSF *tr;
        int omega;      /* omega, in deg/sec */
        {
                multTrsfRot(tr,zunit,omega*rcclGetInterval()/1000.0);
        }
\end{verbatim}
\item[]
This simply uses {\tt rcclGetInterval()} to obtain the system
sample interval (in milliseconds) and converts this into a per-cycle
displacement. 

\item[]{\it Motion Relative Changes}. The variation of a transform can also
be tied directly to the scale value of a motion, that is, the value that
changes from 0.0 to 1.0 as the motion goes from start to finish.
For example, suppose we wish to add a translational displacement
{\tt dp} to a transform over the course of a motion.
Assuming that {\tt dp} has been defined
somewhere, a function that would do this is 
\begin{verbatim}
        dispFxn (tr, arg, mnp)
        TRSF *tr;
        int arg;
        MANIP *mnp;
        {
                float s;

                s = motionScale (getActiveMotionId (mnp);
                tr->p.x = dp.x * s;
                tr->p.y = dp.y * s;
                tr->p.z = dp.z * s;
        }
\end{verbatim}
\item[]
The motion scale is obtained using the {\tt motionScale()} function
called with the ID of the current motion for {\tt mnp}.
Several things about the motion scale value should be noted. First,
if the motion has no explicit time limit, then the scale will always be
0.0. Second, if the motion starts with a non-zero transition time,
then the motion scale will be negative prior to the transition midpoint,
since scale is defined to be 0.0 only at the transition midpoint.
\end{list}

\section{Communicating with the Control Functions}

\subsection{Memory objects}\label{memobj}

The function {\tt convfn()} in the program {\tt zigzag} 
obtained information from the planning level using the application
argument. This works well, provided that no more than one
integer's worth of information needs to be passed. To pass
more information between the planning and control levels,
shared memory facilities are available.

These basically work as follows:
the planning task allocates a piece of
shared memory, the control level routines obtain a pointer to it,
and then everybody can access it. The basic shared memory mechanism
used by RCCL is the RCI {\tt allocMem()} package, which is also
described in the {\it RCI User's Guide}. Its
manual pages appear in the {\it RCI Reference Manual}, instead
of the {\it RCCL Reference Manual}.

Let us assume that the programmer has defined a structure {\tt foo}:
\begin{verbatim}
        typedef struct foo {
                int value1;
                int value2;
        } FOO;
\end{verbatim}
and would like to create an object of this type which is
shared between the planning and control levels.

The object is allocated by the planning level using the call
\begin{verbatim}
        FOO *fp;

        fp = (FOO*)allocMem ("foo", sizeof(FOO), UNDEF);
\end{verbatim}
{\tt allocMem()} takes three arguments: 
a string name for the memory object,
the size of the memory object in bytes, and a
specifier for the memory allocation pool
to be used. This last argument serves the same purpose
as the {\tt pool} argument
in {\tt allocTrans()}; it is typically not too important and is set to {\tt
UNDEF}. 
Also, as with transforms, the name argument is optional.
{\tt allocMem()} allocates the memory, zeros it, and 
returns a pointer to it. Memory can be deallocated
with the call {\tt freeMem()}.

Because the memory returned by {\tt allocMem()} is almost invariably
cast to some structure, there is a macro available to make this
easier:
\begin{verbatim}
        #define ALLOC_MEM(n,t,p) (t*)allocMem(n,sizeof(t),p)
\end{verbatim}
Using this, the allocation of {\tt "foo"} shown above would be written
as 
\begin{verbatim}
        fp = ALLOC_MEM ("foo", FOO, UNDEF);
\end{verbatim}

For another part of the program (such as a control level routine) to
access a shared memory object, it must first obtain a valid pointer
to it. 
Doing this generally requires using a reference function.
This is because the control level may reside on a separate CPU, and
a memory address which is valid at the
planning level may not be valid on a different CPU. To get a valid
pointer, we may use either of the functions
\begin{verbatim}
        void *getMemByName (name)
          char *name;

        void *getMemByAddr (addr)
          void *addr;
\end{verbatim}
{\tt getMemByName()} locates a pointer to the memory using its
{\tt name} as a key (the same name that was specified in the
{\tt allocMem()} call), while {\tt getMemByAddr()} locates
the pointer using the planning level address
(the same one returned by {\tt allocMem()}) as a key.
In practice, the routine {\tt getMemByAddr()} is used more
commonly, because (1) it is faster, and (2) it
relieves us of the need to name the memory object, thereby avoiding
name space collision problems. The planning level address can be
passed to the control routine using either another object or
the routine's application argument\footnote{Time for a confession: we frequently cast pointers to integers
and back for purposes of passing them through the application argument.
The clean way around this would be to either make the application argument
itself a generic pointer, or to make it a union of, say, a generic
pointer and an integer. A change like this may be implemented if it becomes
important, so stay tuned $\ldots$}.

\subsection{Access Collision and Atomic Access}\label{atomic}

When using shared memory to communicate between different tasks,
one frequently runs into the program of access collision: one task may
try to read an object at the same time another task is writing it.
Often enough this is not a problem, since the primary data types which
comprise the object ({\tt float}, {\tt int}, etc.) are 
usually written atomically on most systems and bus architectures for which
RCCL/RCI is implemented. The biggest problem
is that an aggregate which is read back
can be
``out of sync'', {\it i.e.}, while the contents of the individual fields 
are OK, some of them contain ``old''
information and some contain ``new'' information. 
For instance, suppose the planning level reads back the output joint angles in
the {\tt j6} field of the {\tt MANIP} structure, which could be done
simply with the code fragment
\begin{verbatim}
        JNTS jbuf;

        jbuf = *mnp->j6;
\end{verbatim}
While the structure is being copied, it is possible
for the trajectory generator to write new values
to it. This means that the first few joint values may be valid
for one control cycle while the remaining values are valid for 
another control cycle. This sort of thing
may not be a problem, depending
on what the information in the object is being used for.
However, to avoid the situation, there are three primitives available
which may be used to atomically read
and write memory objects which have been created using
{\tt allocMem()}:
\begin{verbatim}
        readMem (mem, buf)
          void *mem, buf;

        writeMem (mem, buf)
          void *mem, buf;

        accessMem (mem, src, dst, size)
          void *mem, src, dst;
          int size;
\end{verbatim}
{\tt readMem()} reads the contents of memory object {\tt mem} into
{\tt buf} (which is assumed to be the same size as the memory object) and
{\tt writeMem()} writes the
contents of {\tt buf} into the memory object {\tt mem}. {\tt buf} is not
assumed to be a memory object and access to it is {\bf not} atomic.
The last routine, {\tt accessMem()}, is the most general: it reads
{\tt size} bytes from {\tt src} into {\tt dst} while locking the memory object
{\tt mem} (which may be either {\tt src} or {\tt dst}, or neither).
It is intended for accessing subfields of an object.

Needless to say, in order for the atomic access primitives to work,
all write operations (and read operations, usually)
on the object in question must use them.
All of the aggregate fields in the {\tt MANIP} structure are updated
by the trajectory generator using calls to {\tt writeMem()}.
They may be read atomically using either {\tt readMem()} or
{\tt writeMem()}, as in
\begin{verbatim}
        JNTS jbuf;
        readMem (mnp->j6, &jbuf);
\end{verbatim}
or
\begin{verbatim}
        float angles[NUM_JNTS];
        accessMem (mnp->j6, mnp->j6->v, 
                   angles, mnp->j6->num*sizeof(float));
\end{verbatim}
The first example copies the entire structure. The second example
is a little faster
because it copies from {\tt j6} only the {\tt v} field entries
corresponding to actual joints.

\subsection{Double Buffering}\label{doublebuf}

We should mention briefly another common technique that can be
used to avoid data consistency problems. This is called double buffering,
and can be used whenever there are several tasks that read an object 
but only one task that writes it. It is conceptually simple, and
generally faster than using the atomic access primitives.
Suppose that the data we are interested in is described by a
type {\tt DATA}. We create an object that contains two versions
of {\tt DATA} and a version number:
\begin{verbatim}
        struct dataObj {
                DATA data[2];
                int version;
        }
\end{verbatim}
The data buffer which is currently in use is indicated by the low bit
of {\tt versionNumber}. When the writing process updates the data, it changes
the buffer that is {\bf not} in use, and then increases the version number:
\begin{verbatim}
        writeData (mem, buf)
        struct dataObj *mem;
        DATA *buf;
        { 
                int new = (mem->version&1)^1;

                mem->data[new] = *buf;
                mem->version++;
        }
\end{verbatim}
The process doing the read uses the data buffer that is in use, and then
checks to see if the version number has increased by more than one.
If it has, it tries again:
\begin{verbatim}
        readData (mem, buf)
        struct dataObj *mem;
        DATA *buf;
        { 
                int idx;
                do
                 { idx = mem->version;
                   *buf = mem->data[idx&1];
                 }
                while ((mem->version&~1) != (idx&~1))
        }
\end{verbatim}
This will work quite nicely in practice, as long as the data is not
updated so frequently as to saturate the system (an update once every RCCL
control cycle would be considered quite infrequent).

\subsection{Locating and Using Other Objects}\label{usingobjects}

Now to divulge what the alert reader was probably beginning to suspect:
Transforms allocated with {\tt allocTrans()} and position equations
allocated with {\tt makePosition()} are also memory objects, completely
analogous to the ones described in the section (\ref{memobj}). They each
have their own access functions:
\begin{verbatim}
        TRSF *getTransByName (name)
          char *name;

        TRSF *getTransByAddr (addr)
          TRSF *addr;

        POS *getPositionByName (name)
          char *name;

        POS *getPositionByAddr (addr)
          POS *addr;
\end{verbatim}
which can obtain pointers to them
given either names or planning level addresses. One reason for giving these
objects separate access routines is to make it possible to separate
their name spaces. For instance, it is
possible to create a memory object using {\tt allocMem()} and a transform
using {\tt allocTrans()} which both have the same string name.

It is also necessary to use a reference routine to get a pointer
to a {\tt MANIP} structure from the control level, even though
these items are not actually implemented as formal objects.
The routines
\begin{verbatim}
        MANIP *getManipByName (name)
          char *name;

        MANIP *getManipByAddr (addr)
          MANIP *addr;
\end{verbatim}
are provided to do this. (NOTE: a valid {\tt MANIP} pointer is
frequently given as an argument to the control level application
functions. It is not necessary to re-reference this pointer.
However, sometimes we want to access the {\tt MANIP}
structure for {\it another\/} manipulator
from within one of these routines, and for this the above routines can be 
useful.)

Transforms also have their own atomic access routines,
analogous to the ones described in section (\ref{atomic}):
\begin{verbatim}
        TRSF *readTrans (tr, buf)
          TRSF *tr, *buf;

        TRSF *writeTrans (tr, buf)
          TRSF *tr, *buf;
\end{verbatim}

To find the name of a transform given a pointer to it, the routine
\begin{verbatim}
        char *getTransName (tr)
          TRSF *tr;
\end{verbatim}
may be used.

Using position equations from the control level is slightly more
complicated. Although
one may access a position equation from anywhere within the RCCL program, the
equation returned will generally not be usable at the control level because
the data structures within the equation itself use pointers whose values are
valid for the planning level. To get around this problem, a routine called
{\tt copyPosition()} is available which makes a local copy of a
position object which is valid for the current memory context.
When the local copy of the position is no longer needed, it can be
deleted using {\tt freePosition()}.
A typical usage paradigm for {\tt copyPosition()} looks something like
this:
\begin{verbatim}
        -- planning level --

           POS *gpos;

           gpos = makePosition (NULL, T6, tool, EQ, b, TL, tool);

        -- control level --

           POS lpos;

           ... gpos is passed down from the planning level ...

           lpos = copyPosition (getPositionByAddr (gpos));

            ... use 'lpos' locally ...
  
           freePosition (lpos); 
\end{verbatim}
The planning level declares the position equation in
the usual way using {\tt makePosition()}. Later,
the control level uses {\tt getPositionByAddr()} to 
get a local reference to equation using the planning level's original
address, and then translates this into valid local equation using
{\tt copyPosition()}. This local copy may then be used
with the routines which do position equation computations,
such as {\tt solveTrans()} or {\tt solveChain()}. Note that
{\tt copyPosition()} copies only the equation itself, not the
transforms that it references.

The ability to access positions from the control level is new and has not
been used much. More information on this feature can be found in the manual
pages. 

\subsection{Synchronizing with the control level}

It is sometimes necessary to synchronize activity between the planning level
and an application-defined control level function.
The basic way of doing this is with flags implemented using shared memory.
Suppose the planning level wants to send a command to a monitor function,
and then wait for the monitor function to respond.
The command can be implemented as a field (which we will
call {\tt com}) in a shared
memory object (which we will assume is called {\tt mem}). Then the
following simple construction may be used:
\begin{verbatim}
        -- planning level --

           mem->com = COMMAND;
           waitWhile (mem->com)

        -- control level function --

           if (mem->com)
            { ... do action ...
              mem->com = 0;
            } 
\end{verbatim}
The planning level just sets the command field and waits for the control level
function to clear it. If it is known that control level will respond during
the next control cycle, then the planning level can use the primitive {\tt
rcclBlock()} instead:
\begin{verbatim}
        mem->com = COMMAND;
        rcclBlock(); 
\end{verbatim}
{\tt rcclBlock()} waits for one entire control cycle to elapse between the
time it is called and the time it returns, and so can be used to effect
single cycle synchronization.

\section{Control Level Support Routines}

\subsection{RCCL system routines}

The following RCCL functions may be called from anywhere with an
RCCL program, but are particularly useful for control level applications.

The system time, in milliseconds, may be obtained with
\begin{verbatim}
        float rcclSysTime();
\end{verbatim}
This does not return ``real time'' in that 
all this really does is return the product of the trajectory generator
sample interval and the number of elapsed control cycles. The system
time does not increase when the trajectory generator is not running
or is paused (as with {\tt rcclPause()}), 
and is reset to 0 every time {\tt rcclStart()} is called.
Applications which need a more genuine time clock can use the
RCI time routine, {\tt rciTime()}, which returns an accurate
timecount (in milliseconds) that is maintained constantly for the
duration of the program.
Applications which change the value of a transform explicitly with time are
encouraged to use {\tt rcclSysTime()}, because it ``stops ticking''
when the system is paused, and so prevents discontinuities in target
positions when the system is unpaused.

The routine
\begin{verbatim}
        getActiveCount(mnp) 
          MANIP *mnp;
\end{verbatim}
returns the {\it count} associated with a manipulator's current motion. The
count starts at 0 (at the beginning of the transition into the motion)and is
increased by one for every control cycle that the 
motion is executed. It can be used to initialize transform functions:
\begin{verbatim}
        function (tr, arg, mnp)
        TRSF *tr;
        int arg;
        MANIP *mnp;
        {
                if (getActiveCount(mnp) == 0)
                 {  ... motion is just starting, do 
                        some special stuff ...
                 }
        }
\end{verbatim}

\subsection{RCI support routines}\label{rcisupport}

Functions executing at the control level do not have access to
UNIX system services, either directly, or indirectly (by calling
library routines such as {\tt printf()} or {\tt malloc()} which use
system services).
To help compensate for this, 
RCI provides a few routines which can be called from the control level
to do diagnostic printing, 
allocate memory, and 
send signals to the main UNIX program.
These include:
\begin{verbatim}
        rciPrintf (format, values ... )
          char *format;

        rciPrintToTerminal (on)
          int on;

        void *rciMalloc (size)
          int size;

        rciFree (ptr)
          void *ptr;

        rciSignal (signum)
          int signum;
\end{verbatim}
{\tt rciPrintf()} uses the same format as {\tt printf()} to
print diagnostic messages
to the console of the CPU running the trajectory task.
If the trajectory task is running on the main UNIX CPU, 
then it is possible to direct the output from {\tt rciPrintf()}
to the user's terminal. This can be arranged by calling the routine
{\tt rciPrintToTerminal()} (with a non-zero argument) at some
point within the program. 

Care should be used when doing printing from the control level, since it
can cause large delays in the execution of the control level code.
In some cases, {\tt rciPrintf()} will block until all
its characters have been output. If an output message is 20 characters 
long, and the console line baud-rate is 9600, then this can cause
(roughly) a 20 msec.\ delay in the control level.

{\tt rciMalloc()} and {\tt rciFree()} are equivalent to the conventional
UNIX routines {\tt malloc()} and {\tt free()}. There is generally a limit
to the total amount of memory that can be allocated, and this may be
as low as 256K.

{\tt rciSignal()} sends the indicated UNIX signal {\tt signum} to the
main program.

For details on all of these functions, the programmer should consult their
manual pages in the {\it RCI Reference Manual}.

\subsection{The Fast Math Library}

Robotics applications make frequent use of the trigonometric functions
{\tt sin()}, {\tt cos()}, {\tt atan2()}, and the function {\tt sqrt()}.
The execution times for these routines can be lengthy (by real-time
standards), particularly if the host machine does not have
hardware support for these functions or the native math library 
does not make use of it.

To help with this problem, RCCL provides a set of routines that do the
same computations using table lookup and interpolation. Execution times
are significantly faster (by as much as 5 or 6 times), and the accuracy
is typically around $10^{-6}$, which is quite tolerable for most
applications.

The available routines are
\begin{verbatim}
        float SIN (x)
          float x;

        float COS (x)
          float x;

        float ATAN2 (x, y)
          float x, y;

        SQRT (x)
          float x; 

        SINCOS (x, s, c)
          float x;
          float *s, *c;
\end{verbatim}
{\tt SIN()}, {\tt COS()}, {\tt ATAN2()} and {\tt SQRT()} do
the same computations as their math library counterparts (except
the arguments and results are in single precision). All angle
values are passed in radians. {\tt SINCOS()}
computes both the sine and cosine of {\tt x}, and returns
the values through the pointers {\tt s} and {\tt c}, respectively.
This routine exists because it is faster to compute both these
quantities together than to compute them separately.

Definitions for these routines are contained in the file
{\tt <fastmath.h>}. The user is encouraged to make use of them.

\section{Program Example: ``rotate''}

This demonstration program uses two transforms bound to circular motions
to create a ``wheels within wheels'' effect. The robot is located
at the center of fictitious circular table which can be made to
rotate about the robot's base. An initial motion is made to a point
tracking a small wheel whose axis is fixed at a point on the larger 
turntable (figure \ref{wheel}). Another motion is then specified for which the
table itself begins to turn and the robot starts tracing out a ``circular
cycloid'' (figure \ref{wheeltable}).
This last motion automatically aborts when the table has traveled
through a certain distance.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/rotate.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/rotateCtrl.560.c}
{\it 
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot
and lives at {\tt rotate.560.c} and {\tt rotateCtrl.560.c}
in {\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 is contained
in {\tt rotate.260.c} and {\tt rotateCtrl.260.c}.

}
\medskip

The first new thing the reader may notice in this example is the
pointer declaration for the type {\tt ROTATE_CTRL_BLK} (\cref{1}),
which is defined in {\tt "manex.560.h"}. 
A memory object of this type is allocated using {\tt allocMem()}
(\cref{2}) and is used to communicate information
to the control level functions. The fields of the structure
contain parameters for the function computations:
{\tt wheelRadius} is the radius of the smaller wheel;
{\tt wheelRPS} is the speed the smaller wheel should spin at 
(in revolutions
per second), and {\tt tableRPS} is the speed the turntable should spin at
(also in revolutions per second) when it starts to move (\cref{3}).

\begin{figure}[tbh]
%\rect 4.25truein by 5truein
\boxedsizedfig{4.25truein}{5truein}%
{\psfig{figure=figs/wheel.idr}}%
{Manipulator tracking a rotating ``wheel'', in the
example program {\tt rotate}.}%
\label{wheel}
\end{figure}

\begin{figure}[tbh]
%\rect 4.25truein by 5truein%
\boxedsizedfig{4.25truein}{5truein}%
{\psfig{figure=figs/wheeltable.idr}}%
{Tracking the ``wheel'' when its
axis of rotation is fixed to a ``turntable'' rotating about the
manipulator's base.}%
\label{wheeltable}%
\end{figure}


The program uses two position equations: {\tt p0},
defined as
\[
{\bf T6} \; = \; {\bf TABLE0} \;\; {\bf P} \;\; {\bf WHEEL}
\]
describes a point on the spinning wheel attached to the larger turntable.
Notice that for this program we have omitted the manipulator
TOOL transform. {\bf TABLE0} describes a rotation about the $z$
axis of the manipulator base frame. {\bf P} defines a translational offset and
flip about the $x$ axis relative to {\bf TABLE0}. 
Both of these transforms are
fixed. {\bf WHEEL} is a moving
transform whose translation values trace out a circle.
Position {\tt p0} assumes that the turntable is
fixed at the initial rotation angle of {\bf TABLE0}. Position {\tt p1},
defined as
\[
{\bf T6} \; = \; {\bf TABLE0} \;\; {\bf TABLE} \;\; {\bf P} \;\; {\bf WHEEL}
\]
is similar, except that the turntable is now set in motion by an
additional (moving) transform {\bf TABLE}.

After the program has turned on the trajectory generator, it sets the wheel
transform in motion with a call to {\tt transEval()} (\cref{4}). The
application argument is specified as 0 because it will not be used.
After moving to the canonical initial position, the robot is told to
do a Cartesian interpolated motion to {\tt p0} and stop there
indefinitely (\cref{5}), during which time it will continue
to track the motion of the wheel (figure \ref{wheel}).
Without waiting 
for this to complete, the program then sets up the motion requests for 
the rest of the program. A call to {\tt transMotionEval()} (\cref{7}) binds
{\bf TABLE} to the function {\tt tableFxn()}, which will be executed whenever 
{\bf TABLE} is contained in the target of the current motion. A request for
a motion of indefinite time to position {\tt p1} is then queued (\cref{8}),
followed by a final motion request
which moves the robot back to the starting position (\cref{9}).

Both the stop at {\tt p0} (\cref{6}) and the motion to {\tt p1} (\cref{8}) are
specified to last indefinitely, 
which means that they must be explicitly aborted
by the program. The stop is canceled by the planning level,
which first waits for the stop request to start executing 
(\cref{10}), waits another 10.0
seconds 
(\cref{11}), and then cancels it with a call to {\tt stopMotion()}.
The motion to {\tt p1} is canceled from within the function
that produces that turntable motion
when the turntable offset angle exceeds $90^\circ$ (\cref{16}).

The control level functions are quite simple.
Each first obtains a pointer to
the control structure using {\tt getMemByName()} and the
name {\tt "rcb"} (\cref{12}, \cref{14}). 
If the memory object is not found, the program is
aborted with a call to {\tt rciAbort()} (see section \ref{rciabort}).
{\tt wheelFxn()} causes the translational component of its transform to trace
out a circle in the $x$--$y$ plane. It increments the current angle of
rotation (\cref{13}) and uses this to compute appropriate values for
{\tt t->p.x} and {\tt t->p.y}. {\tt tableFxn()} causes its transform to rotate
about the $z$ axis. To do this, it increments a rotation angle
and feeds this into the function {\tt rotToTrsf()}.

\section{Program Example: ``pivot''}

This next program makes the robot move through a large arc, with its tool 
pointing along the radius of the arc, as though it were
turning a lever (figure \ref{pivot}). The robot will line itself up with the
lever's initial 
position and then use a functional transform to move it through
$60^\circ$.
Circular arc motions are interesting in that they can sometimes
be accomplished by locating the TOOL frame in a
particular place, rather than by using moving frames. As an
illustration, the program does this to move the ``lever'' back.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/pivot.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/pivotCtrl.560.c}
{\it 
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot
and lives at {\tt pivot.560.c} and {\tt pivotCtrl.560.c}
in {\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 is contained
in {\tt pivot.260.c} and {\tt pivotCtrl.260.c}

}
\medskip

\begin{figure}[tbh]
%\rect 4.25truein by 5truein
\boxedsizedfig{4.25truein}{5truein}
{\psfig{figure=figs/pivot.idr}}%
{Robot ``turning a lever'' about a pivot point, in
the program example {\tt pivot}.}%
\label{pivot}
\end{figure}

The program sets up four position equations {\tt p0}, {\tt pt},
{\tt p1}, and {\tt pd}, defined respectively as
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf B} \;\; {\bf ROT0} \;\; {\bf HANDLE}
\]
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf B} \;\; {\bf ROTY} \;\; {\bf HANDLE}
\]
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf B} \;\; {\bf ROT1} \;\; {\bf HANDLE}
\]
\[
{\bf T6} \;\; {\bf E} \;\; {\bf HANDLE}^{-1} \;=\; {\bf B} \;\; {\bf ROT0}
\]
These describe the various positions of the tip of the lever. The
lever's center
of rotation is located at frame B. To get from there to the tip, we
rotate 
by some amount about the $y$ axis (which is done using either
{\bf ROT0}, {\bf ROTY}, or {\bf ROT1}), 
and then use {\bf HANDLE} to translate
along the $z$ axis. Position {\tt p0} defines the initial position
of the lever, with {\bf ROT0} describing a rotation of $-30.0^\circ$. The
place we 
want to move the lever to is described by {\tt p1}, which is the same as
{\tt p0} except that its rotation angle (given by {\bf ROT1})
is $30^\circ$.
To get from {\tt p0} to {\tt p1} along an arc, the transform {\bf ROTY}
is used.  {\bf ROTY} is bound to a function which varies its rotation angle
from $-30.0^\circ$ to $30.0^\circ$. Position {\tt pd} is used to move
back from {\tt p1} to {\tt p0}. It describes the same target position as
{\tt p0}, except that the TOOL frame is located at the ``pivot'', the
base of the handle, coincident with frame B. In this frame, the
displacement from {\tt p1}
to {\tt p0} is purely 
rotational, and so when the trajectory generator interpolates between the two
positions, it effectively follows the same path (in reverse) as was created by
{\bf ROTY} in the first motion.

The robot first approaches and then positions itself at {\tt p0} 
(\cref{1}). 
An explicit motion time of 12 seconds is established
for the motion to
{\tt pt} (\cref{2}), 
since the trajectory generator would otherwise
compute this based only on the displacement
between {\tt p0} and {\tt pt}, which is initially zero.
{\bf ROTY} is initialized to the value of {\bf ROT0}, and then 
bound to a transform motion function ({\tt pivotFxn()}) that will change the
rotation angle (\cref{3}). The motion to {\tt pt} 
is then requested, which brings the robot to the final position
described by {\tt p1}. This is followed by a small
depart motion away from {\tt p1} (\cref{4}). 

To return to {\tt p0}, the robot is first moved back to {\tt p1}, and then
a motion to {\tt p0} is specified 
using {\tt pd} (\cref{5}) as the target.
As with the
initial motion, the time is explicitly specified; this is done to ensure that
both motions will be completely symmetrical.

Notice that the entire motion sequence for the program is specified without
waiting for any motions to complete; this is done only before the program
exits (\cref{6}).

The function {\tt pivotFxn()} rotates its transform by
$60^\circ$ about the $y$ axis. It records the
initial transform value during the first cycle of the motion; it
senses whether the current cycle is the first cycle by calling {\tt
getActiveCount()} (\cref{7}), which returns 0 only during the first
cycle of the motion.
(We could also have used a static variable in this case, since
{\tt pivotFxn()} is used for only one motion.)
The amount of rotation to apply  is determined from the
motion scale, which is found by using {\tt motionScale()} and the
current motion ID (\cref{8}).

\section{The Joint Bias Functions}\label{jbias}

A recent addition to the RCCL tool kit is {\it joint biasing\/}, which
is an extension of the concept of variable transforms to
joint coordinates. 

Each manipulator has a built in set of joint bias values,
described by the {\tt jbias} field of the {\tt MANIP} structure.
If the user calls the routine
\begin{verbatim}
        jbiasSelect (mnp, on)
          MANIP *mnp;
          int on;
\end{verbatim}
with the argument {\tt on} set to true, then, for each subsequent motion
request, the trajectory generator will add the contents of {\tt jbias}
to the joint values of the motion target. Joint biasing works only for
joint interpolated motions. It may be deselected by calling {\tt
jbiasSelect()} with {\tt on} set to 0.

Like a variable transform, the joint biases may be declared constant,
variable, or bound to an evaluation function, through the following routines:
\begin{verbatim}
        setJbiasConst (mnp)
           MANIP *mnp;

        setJbiasVarb (mnp)
           MANIP *mnp;

        jbiasMotionEval (fxn, arg, mnp)
           int (*fxn)();
           int arg;
           MANIP *mnp;
\end{verbatim}
{\tt setJbiasConst()} sets the joint biases to be {\it constant\/},
which means the trajectory generator uses a private copy of them,
created at the time the motion request was issued. 
This is the default setting. {\tt setJbiasVarb()} instructs the trajectory
generator not to use a local copy of the joint biases, but rather to
constantly track the values in {\tt mnp->jbias}. {\tt jbiasMotionEval()}
binds {\tt mnp->jbias} to the function {\tt fxn} for the duration of the next
motion request. The evaluation function is called by the trajectory generator
using the call format
\begin{verbatim}
        fxn (jbias, arg, mnp)
        JNTS *jbias;
        int arg;
        MANIP *mnp;
        {
                ...
        }
\end{verbatim}
{\tt jbias} is the joint bias values, {\tt arg} is the application
argument provided through {\tt jbiasMotionEval()}, and 
{\tt mnp} is the manipulator containing the joint biases.

Joint biasing is particularly useful in applications involving control of the
robot using joysticks or hand controllers. In these situations, it is often
desirable to map the controller inputs into joint coordinates rather than into
some Cartesian coordinates. Although one can sometimes accomplish this using
functional transforms, the necessary kinematic computations are
quite time consuming, and are not necessarily well defined.

\section{Pausing the System}\label{pause}

A couple of routines are provided to suspend all motions currently being
executed: 
\begin{verbatim}
        rcclPause (accelTime, pauseTime)
          float accelTime, pauseTime;

        rcclResume()
\end{verbatim}
{\tt rcclPause()}
brings all the program's manipulators to rest, for a duration
of {\tt pauseTime} milliseconds; if {\tt pauseTime} is equal to 
{\tt F_UNDEF}, then the pause state will continue indefinitely until
{\tt rcclResume()} is called. The transition time
used to bring the manipulators to rest can be specified in milliseconds
by {\tt accelTime}; otherwise, if this argument 
is given as {\tt F_DEFAULT}, the system will compute the
transition times automatically. As with the function {\tt setTime()}, the
specified transition time is actually one half the total
acceleration time; i.e., it is equal to $\tau$ multiplied by the control
cycle time (see figure \ref{trans}).

When the system is paused, all motion monitors and transform
motion functions are suspended
(i.e., those which are set up with {\tt runMotionFxn()} and
{\tt transMotionEval()}. Permanent monitor and transform functions
continue to execute (monitor functions will be discussed in section
\ref{monitors}).

\begin{quote}
\bf WARNING: One should be careful when pausing the system
with motions underway that depend on real-time sensor inputs.
If large changes in the sensor inputs accumulate while the system
is paused, then the dependent motion(s) might glitch when they are resumed.
\end{quote}

To help the programmer handle the problem stated above, there are
two predicate functions available:
\begin{verbatim}
        rcclIsPaused()

        rcclWasPaused()
\end{verbatim}
{\tt rcclIsPaused()} returns true if the system is currently in a paused
state. Monitor functions can use this and act accordingly. Similarly, {\tt
rcclWasPaused()} returns true if the system was paused during the {\it last}
control cycle; this helps motion monitors or transform motion functions
look out for discontinuities.

Another problem with ``pause glitches'' can occur with functions that depend
explicitly on time. This can be overcome by using {\tt rcclSysTime()}, which
returns a time value which is frozen when the system is paused.

\newpage
\done
