\input rccluginit.latex

\chapter{Interacting with the Environment}

\section{The Low Level RCI Robot Interface}

The present version of RCCL is implemented on top of RCI (Real-time
Control Interface), which is a software system that allows real time
control tasks to be run in a UNIX environment.
RCI also provides a simple joint-level robot and sensor I/O interface,
which RCCL uses internally. Since parts of this interface may be useful to the
RCCL programmer, it is described briefly here. A comprehensive description of
everything in this section may be found in the {\it RCI User's Guide} and
the {\it RCI Reference Manual}.

One reason a programmer may wish access to the RCI interface is
to get at the sensor I/O interface. At the present time,
RCCL does not specifically provide very much in the way of sensor
I/O. It might even be argued that sensor I/O does not
belong in the RCCL package per-se. The disadvantage of having
not done this, however, is that the sensor I/O features that do exist
(using RCI directly) are somewhat ad-hoc. Certain types of sensor
inputs which are clearly robot-specific (such as force/torque measurements
from joint or wrist sensors) may be formally incorporated into RCCL at a later
time.

\subsection{The {\tt RCI\_RBT}\label{rcirbt} Structure}

The RCI robot interface is centered around the {\tt RCI_RBT} structure.
Every robot being controlled by an RCI control task\footnote
{Recall that the trajectory generator is implemented using RCI
control tasks.} is
referenced through one of these.
A pointer to the {\tt RCI_RBT} structure is provided
by the {\tt rbt} field of the {\tt MANIP} structure.

The {\tt RCI_RBT} structure
contains several fields, the most important of which are:
\begin{verbatim}
        JLS *jls;
        KYN *kyn;
        void *var;
        HOW *how
\end{verbatim}

For convenience, pointers to each of these are also provided directly by
fields of the same name in the
{\tt MANIP} structure.

The {\tt jls} field (Joint Level Stuff) points to a data type which
defines an extensive set of fixed joint level parameters for the robot.
It is a veritable grab-bag of all sorts of things
such as the number of joints, the type of each joint, the
gear ratios, joint value limits, maximum velocities, calibration
information and vectors, desired nominal
velocities, etc. It is, in effect, a database of joint level robot
parameters.

The {\tt kyn} field points to the robot's {\tt KYN} structure, which
contains various {\it kinematic} and {\it dynamic}
level parameters for a particular
robot. 

The {\tt KYN} structure is defined as follows:
\begin{verbatim}
        typedef struct _KYN {
           GEN_KYN *gen;
           void *ext;
        } KYN;
\end{verbatim}

The {\tt gen} field points to a substructure of type
{\tt GEN\_KYN}, and contains parameters generic to
most serial link robots. This includes such things
as the ``A'' matrix parameters, link inertias and masses
(currently being added), and joint friction coefficients. 
It also contains pointers to routines that do robot specific
kinematic and dynamic computations.

The {\tt ext} field points to an data area which defines
extra parameters specific to the robot's class. This can include
things such as precomputed terms for enhancing
the efficiency of the kinematic computation functions.
The type associated with this data area varies 
from one robot class to another, and takes a name of the
form {\tt <CLASS>_KYN}. The one for the PUMA
robots, defined in {\tt puma\_kynvar.h}, is called {\tt PUMA\_KYN}.
Notice that the {\tt ext} field
is declared to be {\tt void*}, so that it may be handled 
independantly of the robot class. 

The {\tt var} field points to the robot's {\tt VAR}
structure, which is a hook to help do real-time
computations. It is a ``scratch pad'' in which
terms which are generated during a particular computation
may be remembered for use in another computation. Like the {\tt ext}
component of the {\tt KYN} structure, 
the actual definition depends on the robot class, and so
it is generally referenced using a generic pointer. Typical things that are
placed in the {\tt VAR} structure include the sines and cosines
of the joint angles and coefficients of the Jacobian. The {\tt VAR} structure
for PUMA robots is defined by the type {\tt PUMA_VAR}. It is unlikely that
this structure will be of much initial interest to an RCCL
programmer.

The {\tt HOW} structure is a blackboard containing robot state and sensor
I/O information. It defines a large number of fields, not all of which are
necessarily implemented on a particular system.
The information in the {\tt HOW} structure is updated once every control cycle
by the RCI interface.
More information on {\tt HOW} is given in section \ref{how}.

More information on the {\tt JLS}, {\tt KYN}, and {\tt VAR} structures
can be found in the ``RCI Reference Manual'' and the ``RCI User's Guide''.

\subsection{Other ways to get {\tt JLS}, {\tt KYN}, and {\tt VAR}}

It is not necessary to get the {\tt JLS}, {\tt KYN}, or {\tt VAR} structures
for particular robot from its {\tt MANIP} structure. Indeed, this may not be
possible if the robot in question is not under explicit RCCL control.
Pointers to these structures are necessary in order to use the
kinematic and dynamic functions described in section \ref{kinematic}.

Pointers to the {\tt JLS} and {\tt KYN} structures can be obtained by calling
the routines {\tt getJls()} and {\tt getKyn()} with the name of the robot in
question:
\begin{verbatim}
        #include <rci.h>

        JLS *jls;
        KYN *kyn;
        int kynExtSize;

        jls = getJls ("nameOfRobot");
        kyn = getKyn ("nameOfRobot", &kynExtSize);
\end{verbatim}
The second argument to {\tt getKyn} is an optional integer pointer for
returning the size of the structure associated with the 
{\tt ext} field. Both of these
routines, and the pointers they return, can only be used from the
RCCL planning level.

To access {\tt JLS} and {\tt KYN} structures from the control level
as well as the planning levels, the routines
\begin{verbatim}
        rciGetJls (name)
        rciGetKyn (name, sizep)
\end{verbatim}
should be used instead. For any particular robot, each of these routines must
be called once initially from the planning level, since the first call
accesses file information. On Sun4 and Micro-VAX systems, the first call
should also be made when the control level is {\bf not} running, because
of difficulties with the memory locking code.

{\tt VAR} structures must be allocated by the application program.
The necessary size can be be determined using the routine
{\tt getVarSize()}:
\begin{verbatim}
        void *var;

        var = malloc (getVarSize("nameOfRobot"));
\end{verbatim}
If called from the RCCL control level, {\tt rciMalloc()} should be used
in place of {\tt malloc()}. If the structure is to be shared by both the
planning and control levels, then {\tt allocMem()} should be used.

Once allocated, it is often necessary to
instantiate the {\tt VAR} structure with values for a particular robot
position; see section \ref{kinematic}.

More information on these routines
can be found in the ``RCI Reference Manual'' and the ``RCI User's Guide''.

\subsection{The {\tt HOW}\label{how} Blackboard}

Few of the {\tt HOW} structure fields are likely to be of immediate interest
to an RCCL programmer; those that are are
described here. Some which relate directly to the robot are
\begin{verbatim}
          int enc[MAXJNTS];
        float encVel[MAXJNTS];
        float jtorque[MAXJNTS];

         char torqueOK;
\end{verbatim}
{\tt enc} and {\tt encVel} give the robot's current position and velocity,
as read back from the robot controller, in
terms of encoder counts. Angle level information is also available, but this
can generally be obtained from the fields of the {\tt MANIP}
structure. {\tt jtorque} gives the current force/torque
values on each joint. The system must be explicitly asked to
update the {\tt jtorque} field (see the next section), but some systems
(or robots!)\ do not support this capability. If {\tt jtorque}
is being updated, and the information in
it is valid for the current cycle, the {\tt torqueOK} field is set true.

Some fields related to sensor I/O include:
\begin{verbatim}
         long options;
         char numAdc;
         char numPioIn;
         char numPioOut;

         long pendantInput[HOW_PENDANT];
        float forceInput[6];
          int jstickInput[6];
          int adcInput[HOW_MAXADC];

       ushort pioInput[HOW_MAXPIO];

         char pendantOK;
         char forceOK;
         char jstickOK;
         long adc;
\end{verbatim}
{\tt options} is a bit mask describing which of the various sensor
I/O features are implemented. Codes of interest here
include {\tt TORQUE_AVAIL} (joint torque information is available),
{\tt PENDANT_AVAIL} (teach pendant information is available),
{\tt FORCE_AVAIL} (wrist force sensor information is available),
{\tt JSTICK_AVAIL} (input from a multi-DOF joystick is available).
{\tt numAdc} is the number of analog-to-digital converter channels
available, {\tt numPioIn} is the number of parallel input ports,
and {\tt numPioOut} is the number of parallel output ports. Input
from the teach pendant, force sensor, joystick, and ADC channels
can be turned on and off using special commands (see the next section).
When valid data is read back from each of these, it is placed
in the appropriate field
{\tt pendantInput}, {\tt forceInput},
{\tt jstickInput}, or {\tt adcInput}, and the corresponding
field {\tt pendantOK}, {\tt forceOK}, {\tt jstickOK}, or {\tt adc}
is set valid.
Parallel port input information is always available through
the field {\tt pioInput}.

{\tt HOW} structure
information may be read back from any part of an RCCL program,
although there is currently no way to do this
atomically (section \ref{atomic}), unless the read is done from the
control level on the same CPU that is running the trajectory
generator for the manipulator.

The reason the {\tt HOW} structure was implemented using shared memory
was to make access to it very fast. Historically, this was very important
because the original RCCL systems were implemented on VAXen, where the
overhead associated with a subroutine call can be particularly gruesome.
The inability to read information from it atomically could be resolved
by double buffering, since the structure is updated at a regular interval.
Both issues could, and probably should,
be fixed by wrapping access to the {\tt HOW} structure
fields in macros.
For more detailed information on the {\tt HOW} structure, the reader should
consult either the manual page or the {\it RCI User's Guide}.

\subsection{Robot and I/O Commands}

The ``other half'' of the RCI robot interface is a large set of command
macros that control the robot and attached sensor devices.
Like the {\tt HOW} structure, these macros can be called from anywhere within
the RCCL program. What each macro actually does is store the command
information away in a private buffer that is flushed by the RCI interface
once every control cycle. Each macro is called with a pointer
to the {\tt RCI_RBT} structure, along with whatever other arguments are
appropriate.
Since RCCL uses the command interface itself, care should be taken to
avoid conflicts. Some of the commands likely to be of interest to an
RCCL programmer
and unlikely to conflict with the trajectory generator are:
\begin{verbatim}
        SET_TORQUE_READ (rbt, on)
        SET_FORCE_READ (rbt, on)
        SET_JSTICK_READ (rbt, on)
        SET_ADC_CHAN (rbt, mask)
        PUT_PIO (rbt, num, value)
\end{verbatim}

{\tt SET_TORQUE_READ()} enables reading of the joint torque values
into the {\tt jtorque} field of the {\tt HOW} structure. Likewise, {\tt
SET_FORCE_READ()} and {\tt SET_JSTICK_READ} enable reading of the wrist/force
torque sensor and the multi-DOF joystick into the {\tt forceInput} and
{\tt jstickInput} fields of the {\tt HOW} structure. {\tt SET_ADC_CHAN()}
sets
a mask describing which ADC channels should be activated for reading into
the {\tt adcInput} field of the {\tt HOW} structure. Finally, {\tt PUT_PIO}
places
the bit pattern {\tt value} into parallel output port {\tt num}.
The value which is set may be read back from the {\tt pioOutput} field of the
{\tt HOW} structure.

\section{Kinematic Computation Functions}

\subsection{Routine Descriptions}\label{kinematic}

RCI provides routines for doing various types of
kinematic and dynamic computations, such as forward and inverse kinematics,
forward and inverse Jacobian mappings, etc. 
Each routine takes as one of its arguments a pointer to the robot's
{\tt KYN} structure. Some routines require pointers to the
{\tt VAR} and/or {\tt JLS} structures as well. The following
functions are currently defined:
\begin{verbatim}
        fwdKinematics (t6,c,j6,kyn,var)
        invKinematics (j6,t6,c,refj,kyn,jls,var)
        solveConf (c,j6,kyn)

        fwdJacob (dc,dj,j6,kyn)
        invJacob (dj,dc,j6,kyn,epsilon)
        fwdJacobT (t,f,j6,kyn)
        invJacobT (f,t,j6,kyn,epsilon)

        gravload (t,j6,kyn)

        strToConfig (c,s,kyn)
        configToStr (s,c,kyn)
\end{verbatim}
{\tt fwdKinematics()} computes {\tt t6} and a
configuration bit-code {\tt c} from the joint values {\tt j6}. It
uses information provided by the {\tt KYN} structure {\tt kyn}
and updates the sine/cosine information in the {\tt VAR} structure
{\tt var} if that argument is not {\tt NULL}.  {\tt invKinematics()} is the
most complicated routine: it computes  {\tt j6} given {\tt t6}, a
configuration bit-code {\tt c}, and a set of reference joint values
({\tt refj}) that it uses to help resolve singularities and joint angle
redundancies. It uses information in the {\tt kyn} and {\tt jls} structures
and, if {\tt var} is not {\tt NULL}, updates the sine and cosine
terms in that.  {\tt solveConf()}
computes only the robot configuration {\tt c} from the joint values
{\tt j6}.

The Jacobian routines each apply one of the four principal Jacobian
calculations to an input vector.
Each uses information in the {\tt kyn} structure as well as the current
robot joint values {\tt j6}.
Ecah routine computes,
respectively,
\[
{\bf dc} \;=\; {\bf J} \; {\bf dj}
\]
\[
{\bf dj} \;=\; {\bf J}^{-1} \; {\bf dc}
\]
\[
{\bf t} \;=\; {\bf J}^T \; {\bf f}
\]
\[
{\bf f} \;=\; {\bf J}^{-T} \; {\bf t}
\]
The variable {\tt dc} is of type {\tt DIFF} and
{\tt f} is of type {\tt FORCE}. {\tt dj} and {\tt t} are simply
vectors of floats.

The routine {\tt gravload()} computes the gravity loading torques {\tt t}
given a {\tt kyn} structure and the robot's
joint position {\tt j6}. No other dynamics routines are implemented at this
time. 

{\tt strToConfig()} takes a string representation {\tt s} of the robot's
configuration and returns the corresponding bit code {\tt c}. {\tt
configToStr()} performs the inverse operation.

Arguments for the above routines which correspond to sets of joint values
are defined to be of type {\tt float*}.
This is because the ``level'' at which these routines are defined is
below that of RCCL and the definition of the {\tt JNTS} data type.
When using a {\tt JNTS} data type with these routines,
the {\tt v} field should be used for the argument.

More detailed information about these routines, and what they do in the
event of errors, can be found by consulting their
respective entries in the ``RCI Reference Manual''.

Some of these routines make use of terms, such as the sines and cosines of the
joint angles, or entries in the Jacobian matrix, which are constant for a
particular robot position. It may be computationally more efficient
to compute these terms only once if several routines are called
for a specific robot position. Because of this, some routines have equivalent
routines, whose names end in {\tt Var}, which take the robot {\tt VAR}
structure as an argument in place of the joint values. 
These routines currently are:
\begin{verbatim}
        fwdJacobVar (dc,dj,var,kyn)
        invJacobVar (dj,dc,var,kyn,epsilon)
        fwdJacobTVar (t,f,var,kyn)
        invJacobTVar (f,t,var,kyn,epsilon)
        gravloadVar (t,var,kyn)
\end{verbatim}

When these routines are called, the {\tt VAR} structure
must be valid for the current robot position. 
The {\tt VAR} structure associated with the robot's
{\tt mnp} structure is automatically maintained by the RCCL trajectory
generator, and so will be valid whenever the trajectory generator is running.
Programs should not modify the contents of this particular {\tt VAR} 
structure. 

Otherwise, if the {\tt VAR} structure is allocated by the
user, then it is necessary to explicitly update it for a given
joint position.
There are two routines
for doing this:
\begin{verbatim}
        updateVar (var,j6,kyn)
        updateVarXsincos (var,j6,kyn)
\end{verbatim}
{\tt updateVar()} computes all the information in the
{\tt VAR} structure based on a given set of joint values {\tt j6}
and information in the {\tt KYN} structure. {\tt updateVarXsincos()}
does the same thing, but does not compute the sine and cosine terms
that may optionally be set by the routines {\tt fwdKinematics()} and {\tt
invKinematics()}. 
It is only necessary to update the {\tt VAR} structure when the
robot position changes (which typically happens once per control cycle).
The following call sequence would be valid:
\begin{verbatim}
        updateVar (var,j6,kyn);
        fwdJacobVar (dc,dj,var,kyn);
        gravloadVar (load,var,kyn);
\end{verbatim}

All of these functions are implemented internally by routines specific to
each particular robot class. Pointers to the implementation routines are
contained in the {\tt KYN} structure itself. 
The internal implementation routine for a particular robot class takes
the same name as the external routine, only with the first letter capitalized
and the robot class name added in front. For example, the routine
to implement {\tt fwdKinematics()} for the PUMA is called
{\tt pumaFwdKinematics()}.

These routines are meant to be somewhat ``robot independent''. In fact,
there is still some robot dependency with regard to the inverse routines,
since inverses are only well defined for robots with six degrees of
freedom or less. For robots with more degrees of freedom, the interface
functions would probably need to be generalized to allow the specification of
strategies for resolving the redundancy.

\subsection{Example Program}

The following stand alone program illustrates how the
above routines can be used directly to compute inverse kinematics
for robots supported by RCI.
\begin{verbatim}
   #include <rccl.h>
   #include <rci.h>
   
   char **argvec;

   main(argc, argv)
   int argc;
   char *argv[];
   {
           float ang[MAXJNTS];
           float roll, pitch, yaw;
           unsigned long confcode;
           char confstr[9];
           int status;
           TRSF t6;
           JLS *jls;
           KYN *kyn;

           argvec = argv;
           argc--; argv++;
           if (argc != 1)
            { usage (-1, "%s <robot-name>\n", argvec);
            }
           if ((jls = getJls (*argv)) == NULL)
            { printErrors ();
              exit (-1);
            }
           if ((kyn = getKyn (*argv, (int*)0)) == NULL)
            { printErrors ();
              exit (-1);
            }
           while (1)
            { if (scanf ("%f%f%f%f%f%f%s", &t6.p.x, &t6.p.y, &t6.p.z,
                          &roll, &pitch, &yaw, confstr) != 7)
               { exit (0);
               }
              rpyToTrsf (&t6, roll, pitch, yaw);
              strToConfig (&confcode, confstr, kyn);
              status = invKinematics (ang, &t6, confcode, (float*)0, 
                                      kyn, jls, (void*)0);
              if (status & (KYN_JOINT_LIMIT | KYN_CANT_REACH))
               { printf ("Out of range\n");
               }
              else
               { radianToDegree (ang, ang, jls);
                 printVf ("joints: %9.3f\n", ang, jls->numJ); 
               }
            }
   }
\end{verbatim}

The program gets the name of the robot from the argument list. 
It then calls {\tt getJls()} and {\tt getKyn()} to obtain pointers
for the robot {\tt JLS} and {\tt KYN} structures.
The rest of the program is a loop which reads in a Cartesian
position and a desired robot configuration, and applies the inverse
kinematic routine. The Cartesian position is 
described by $x$, $y$, and $z$ translations and by roll, pitch, and
yaw rotations, which are converted to the {\tt TRSF} structure {\tt t6}.
The desired robot configuration is read as a string and is converted
to a bit code using {\tt strToConfig()}. Both {\tt t6} and the configuration
code are handed to {\tt invKinematics()}, which tries to solve for
the joint values and return them in {\tt ang}. The fourth argument
to {\tt invKinematics()} is optional and is omitted here. If specified, it
gives a set of joint values that are used to resolve joint angle
redundancies (caused by the fact that a joint can have more than $360^\circ$  
of range). If omitted, the joint mid-range values are used instead
(these are obtained from the {\tt JLS} structure).

{\tt invKinematics()} returns a status value which is used to see
if the routine succeeded. Although this example does not do so,
a status value that indicates failure 
can be analysed to yield detailed information about what went wrong.
If the inverse kinematics routine succeeds, then the revolute
joint values are converted to degrees using {\tt radianToDegree()} and the
result is printed out.

\section{Control Level Routines and the Trajectory Generator}

\subsection{Monitor Functions}\label{monitors}

In addition to transform functions, an
RCCL program can establish
{\it monitor} functions, which are also executed by the trajectory generator,
and can do things ranging from general sensor computations
to the monitoring of cancellation conditions for motions.

There are two types of monitor functions, similar in concept to
the two types of transform functions: {\it permanent} monitor
functions, which, once created, are executed until
they are explicitly deleted, and {\it
motion} monitor functions, which are executed
only for the duration of a particular motion.

Permanent monitor functions are created and deleted with the routines
\begin{verbatim}
        runMonitorFxn (mnp, fxn, arg)
          MANIP *mnp;
          int (*fxn)();
          int arg;

        deleteMonitorFxn (mnp, id)
          MANIP *mnp;
          int id;
\end{verbatim}

{\tt runMonitorFxn()} instructs the trajectory generator
to call {\tt fxn} once every control cycle, with the
application argument {\tt arg}, in the following format:
\begin{verbatim}
        fxn (arg, mnp)
        int arg;
        MANIP *mnp;
        {
           ...
        }
\end{verbatim}
Function execution begins immediately (provided the trajectory generator is
running).
{\tt runMonitorFxn()} returns an ID value which can be used
to cancel the monitor function with {\tt deleteMonitorFxn()}.
Both {\tt runMonitorFxn()} and {\tt deleteMonitorFxn()}
require a pointer to a manipulator; this specifies
which trajectory task should be used to run the monitor
(recall that for multiple CPU systems, one trajectory task is created per CPU,
and each {\tt MANIP} is controlled by a particular task).
A program may run up to {\tt MAX_MONITOR_FXNS} monitors
at once.

A motion monitor function is run for the duration of
a particular motion. It may be
established with the call
\begin{verbatim}
        runMotionFxn (mnp, fxn, arg)
          MANIP *mnp;
          int (*fxn)();
          int arg;
\end{verbatim}
The trajectory generator will then execute {\tt fxn} once every control cycle
for the time span of the next requested motion on {\tt mnp}.
The monitor
is called using the same arguments as for permanent monitors.
\begin{verbatim}
        fxn (arg, mnp)
        int arg;
        MANIP *mnp;
        {
           ...
        }
\end{verbatim}
Execution starts at the beginning of the transition into
the motion, and ends at the beginning of the transition out
of the motion.
Up to {\tt MAX_MONITOR_FXNS} motion monitors may be specified for any
given motion.

Monitor functions may communicate with other parts of the program using the
same shared memory primitives that are available to the rest of the system
(section (\ref{memobj})).

\subsection{Trajectory Generator Computation Sequence}\label{sequencing}

When an application is created where several different
transform and monitor functions are executing together, it may be
important to know the order in which the various executions
take place.
This section describes the computation sequence used by the
trajectory generator for each control cycle.

\begin{figure}[tbhp]
\boxedfullpagefig
{\psfig{figure=figs/controlcycle.eps}}%
{Default sequence of operations
carried out by each trajectory
task, once per control cycle.}%
\label{controlcycle}%
\end{figure}


\begin{figure}[tbh]
%\rect 6truein by 4.5truein%
\boxedsizedfig{6truein}{4.5truein}%
{\psfig{figure=figs/rendezvous.eps}}%
{Rendezvous points between trajectory tasks
executing on separate CPUs.}%
\label{rendezvous}%
\end{figure}


We will describe the default behavior first (see figure
\ref{controlcycle}). Each trajectory
task begins by exchanging data with all the robots
which it is controlling: for each robot, state and sensor information is
read in and the trajectory setpoints
and commands which were computed and accumulated during the
{\it previous\/} control cycle are written out.
The rendezvous operation which follows the robot communication
will be described momentarily.
Next, all the control computations are performed:
for each of the task's {\it manipulators\/}\footnote{We say manipulator instead of robot because it is possible to create
virtual manipulators which are not attached to any real robot. This is
described in section \ref{virtual}.}
all the permanent monitors are called, followed by all the
motion monitors. The current motion count is then increased,
and if it is time to start
a new motion request, that is done and
the motion count is set to zero. All the functional transforms
associated with the current (and perhaps new) motion
segment (the {\tt transMotionEval()} bindings) are then evaluated.
The permanent functional transforms
are evaluated afterwards.
Finally, the trajectory generator combines all this information
and solves for the output setpoints of each manipulator.

\begin{figure}[tbh]
%\rect 6truein by 4truein%
\boxedsizedfig{6truein}{4truein}%
{\psfig{figure=figs/lag.eps}}%
{Default sequence used by the trajectory
task for communicating with the robots.}%
\label{lag}%
\end{figure}


\begin{figure}[tbh]
%\rect 6truein by 4truein%
\boxedsizedfig{6truein}{4truein}%
{\psfig{figure=figs/nolag.eps}}%
{Robot communication sequence with no control lag.}%
\label{nolag}%
\end{figure}


When several trajectory tasks are running on different CPUs,
the computations described here will overlap. While this can
certainly be desirable, it may cause some ambiguities about what was
computed when. To offset this problem, all the trajectory tasks rendezvous
with each
other at two prescribed points during each control cycle (figure
\ref{rendezvous}). The first rendezvous takes place after
each robot's state
and sensor data has been read
in, and ensures that each task will see the same
state and sensor information for each robot.
The second rendezvous takes
place after all the application computations have been performed.
At this point, all the information associated with moving motion
targets has been computed, and the rendezvous ensures that the same
values will be seen by each task.

In cases where several monitor or transform functions are defined
simultaneously, they are evaluated in the same order that they were
set up in. In other words, for the following example,
\begin{verbatim}
        extern foo();
        extern bar();

        runMonitorFxn (mnp, foo, 0);
        runMonitorFxn (mnp, bar, 0);
\end{verbatim}
monitor function {\tt foo} will be executed before function {\tt bar}.
For the transform motion functions set up with {\tt transMotionEval()}, the
order of evaluation is based on the transform's counterclockwise location
within the position equation, starting with the transform closest to {\bf T6}.

The reader will notice that by default, the trajectory task
reads data in from the robot, writes out setpoints and commands from the
previous cycle, and then computes the setpoints for the next control cycle.
This is done to allow parallel operation with the low level robot controller.
Typically, the robot controller collects the robot state and sensor
information, sends this to the trajectory generator, and receives back a set
of setpoints and commands which it then executes (figure \ref{lag}).
The computation of the next setpoint values by the trajectory task can then
overlap with the execution of the current setpoint values by the robot
controller. This can save time, particularly if the robot controller is slow
(which was true historically). However, it also introduces a one-cycle delay
in the trajectory computation process, which can be detrimental in
feedback situations where the setpoints depend on the state and sensor
information.

It is possible to have the robot setpoints sent to the controller
at the {\bf end} of each control cycle, rather than at the beginning
of the next control cycle. To do this, the routine
\begin{verbatim}
        rcclTightControlLoop (on)
          int on;
\end{verbatim}
should be called, with {\tt on} set true,
at some point when the trajectory generator is switched off.
Setpoints will then be sent to the robots during the same cycle
that they are
solved for, as shown in figure \ref{nolag}.

\section{Sensor Integration}

An important feature of RCCL is its ability to easily combine control
of the robot with a variety of sensor inputs.
The robot behavior can be influenced
by modifying a motion's target positions or canceling the motion
altogether.
If the target position is modified in advance of the motion,
this is called ``presetting the world model''; such sensor
integration occurs at the task level.
Initializing a transform is a special case of this.
If the target position is modified while the motion
is in progress, this is called ``tracking''.
If sensor monitors are set up to
cancel a motion on certain conditions,
this is called a ``guarded motion''.
Executing a guarded motion usually implies some uncertainty
about the world model; correspondingly, if the guarded motion makes
``contact'', then the environment is known with greater
certainty than before, and it may be desirable to
update the world model accordingly.

\subsection{Task Level Integration}

This section will present a demonstration program
that simulates the integration of
RCCL with a computer vision system.
Assume that a camera has been attached to link 4 of
a PUMA robot.
The computer
vision system is simulated by a function {\tt snapshot()},
which causes a picture to be taken of the scene
and stored, and by a function {\tt getobj()}, which
extracts the position and orientation
of an object in the camera coordinate frame.
The task
is programmed in such a way that the ``processing'' done by {\tt getobj()}
overlaps with the motion of the arm.
The strategy used
consists of moving the manipulator toward a position
where the object is expected to be in the camera's field of view.
The program is synchronized so that the picture is taken at a given
point in the trajectory.
At the instant the picture is taken, the manipulator's {\bf T6} value
is recorded, and from this the camera coordinate frame can be computed.
Knowledge of the camera coordinate frame, plus the object position
returned in camera coordinates, provides all the information
necessary to approach the object, grasp it, and take it somewhere else.

\newpage
\cverbatimfile{exampleCode/pickAndDrop.560.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot,
and lives at {\tt pickAndDrop.560.c} in
{\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 is contained
in {\tt pickAndDrop.260.c}

}
\medskip

The program makes use of
three position equations:
the position {\tt look},
where the object is expected to be seen
by the camera, the position {\tt get},
at which the object should be grasped,
and the position {\tt put}, where the object should be placed.

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/lookGraph.idr}}%
{Transform graph for position ``look''.}%
\label{lookGraph}%
\end{figure}


The position equation {\tt look} expresses
the ordinary transform graph shown in figure \ref{lookGraph}.
{\bf Z} is a reference frame at the base of the manipulator pedestal,
{\bf E} is the usual end effector frame, and {\bf EXPECT} is the location,
with respect to
the pedestal base,
where we expect to find the object in the camera's field of view.

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/putGraph.idr}}%
{Transform graph for position ``put''.}%
\label{putGraph}%
\end{figure}


The position equation {\tt put} also expresses
an ordinary transform graph (figure \ref{putGraph}).
{\bf DROP} is the location, relative to the manipulator base, where the object
is to be dropped.

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/getGraph.idr}}%
{Transform graph for position ``get''.}%
\label{getGraph}%
\end{figure}


The position equation {\tt get} expresses
a slightly more complicated transform graph (figure \ref{getGraph}).
It uses the additional transforms {\bf COORD}, which
locates the base frame to which the camera is mounted
(in this case link 4 of the arm), {\bf CAM}, which describes the
camera view frame relative to the camera base frame, and {\bf O}, which
describes the position of the object in relative to the
camera view frame.

\begin{figure}[tbh]
%\rect 5truein by 4truein%
\boxedsizedfig{5truein}{4truein}%
{\psfig{figure=figs/complexGraph.idr}}%
{More complex description of position ``get''.}%
\label{complexGraph}%
\end{figure}


Once all the component transforms of {\tt get} are known, the manipulator may
be moved to the pickup location. {\bf CAM} and {\bf E} are given, and
{\bf O} is returned
by the ``vision software'' inside {\tt getobj()}. How do we find
{\bf COORD}? Note that {\tt get} is really just a subgraph of the
(nicely complicated) graph shown in figure \ref{complexGraph}.

The camera is mounted on link 4 of the robot, and so
{\bf COORD} is equivalent to
the manipulator's ${\bf T4}$ transform, which is the
product of the robot's first 4 ``A'' matrices (see section \ref{poseqns}).
${\bf T4}$ may be cumbersome
to compute directly, but this is not necessary: instead, we can compute the
transform ${\bf U5}$ which maps from link 4 to link 6 (and is the product
of the robot's last two ``A'' matrices. From the graph
it is obvious that
\[
{\bf COORD}\;=\;{\bf T6}\;\;{\bf U5}^{-1}
\]

${\bf U5}$ can be computed quite easily given joint angles 5 and 6; the
program does
this using the routine {\tt computeU5()}. Notice that
the sines and cosines of the two angles are computed
using {\tt SINCOS()}, which
is part of the RCI {\it fastmath} library.

Looking now at the program itself, we see that it increases the
manipulator speed
for dramatic effect (\cref{1}), opens the hand, and then enters into
a loop where it will ``pick and drop'' objects until {\tt getobj()}
says that it can't find any more.

Once the robot has been told to move to {\tt look}, the program
waits on the corresponding motion ID until the robot is 70\% of the way there
(\cref{3}). At this point the camera is told to take a picture
(\cref{4}), and the current {\bf T6} and joint angles values are read
back from the manipulator structure (\cref{5}).
{\tt readTrans()} and
{\tt readMem()} are used to guarantee that the data is read
atomically \footnote{although it will {\bf not} guarantee that {\bf T6} and
the joint angles are
consistent with respect to one another; however, this is unlikely
to be a problem because the robot is not moving
very fast. If it did present a problem, we could grab only the joint angle
values, and compute {\bf T6} using the forward kinematics routines.}.
${\bf U5}$ is computed using the read back angle values and the recorded
{\bf T6} value is post
multiplied by its inverse (\cref{6}) to get
{\bf COORD}. Finally, {\tt getobj()} is called to either
report the object position or indicate that it couldn't find
anything. Notice that {\tt getobj()} has the remainder of the travel time
to the target {\tt look} in which to compute the value of {\bf O}. While it is
unlikely
that a complicated vision function could profit much from this extra
time, this does illustrate how the asynchronous
nature of the motion requests can be put to use.

The rest of the loop (\cref{7}) issues the commands for the robot to
approach the object, grasp it, and take it away and drop it.
Notice how the opening and closing of the hand is coordinated with
the motion requests using motion IDs (\cref{8}, \cref{9}).

The function {\tt getobj()} constructs a simulation of {\bf O} using
a random number generator and a few well chosen transform manipulator
routines.

As a side note, the reader might notice that we have not assigned
names to any of the position equations or transforms
in the program, and this will be true of the remaining examples as well.
Names for these are only necessary when they are used by calling either
{\tt getPositionByName()} or {\tt getTransByName()}. Memory
objects will also be unnamed for the same reason. The use of
names in the previous examples was for illustrative purposes only.

\subsection{Tracking}

This next example illustrates {\it tracking}, which is what happens when
functionally defined transforms are updated based on sensor readings.
The program itself is very simple: the $xy$ translation values of a
particular
transform are made to follow the readings from a dual-channel
potentiometer.

When connecting sensor inputs directly to transform values, one typically
uses either position tracking, in which the transform coordinates
are constructed directly from the input values, or velocity tracking,
in which the inputs define an incremental offset which is applied to the
existing transform value.
Position tracking has the advantage that
no errors are accumulated, but the resulting motion can be very rough unless
some sort of smoothing is applied. Velocity tracking is much smoother but can
result in drifting unless the input device has a well defined dead band.
The demo program here uses absolute position tracking.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/track.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/trackCtrl.560.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot
and lives at {\tt track.560.c} and {\tt trackCtrl.560.c}
in {\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 is contained
in {\tt track.260.c} and {\tt trackCtrl.260.c}. If the program discovers
that the system does not have the correct sensor inputs,
the tracking function just
punts and simulates them.

}
\medskip

The program uses only a very simple position equation
{\tt p1}, containing a base transform {\bf P}, which is
set to correspond to the robot's starting position, and
a tracking transform {\bf TRACK}.
The structure {\tt TRACK_CTRL_BLK} (\cref{1}) is used to communicate between
the planning level and the tracking function. It contains the following
control fields:
{\tt compute}, which ``turns the function on'', {\tt gain}, a constant
by which the sensor input values are multiplied,
{\tt vlimit}, a tracking velocity threshold (in millimeters per
second), {\tt mnpAddr}, the planning level address of the
{\tt MANIP} structure which is used to reference the {\tt MANIP}
structure from the control level, and {\tt tracking}, a field which is used
internally by the tracking function.
The fields {\tt xval} and {\tt yval} are set by the tracking
function to
the current $x$ and $y$ target values being tracked.
The tracking function is set running with a call to {\tt transEval()}
(\cref{2}). This is followed by a call to {\tt rcclBlock()}, so
that we know that
the tracking function has executed at least once. The robot is then requested
to move to the tracking position and stay there indefinitely while the
planning task reads and prints out the $x$ and $y$ coordinates
as determined by the
tracking function (\cref{4}).
There is no explicit quit command for this program;
the usual interrupt character will do quite nicely\footnote{Notice that this means we do not call {\tt rcclRelease()} at the
end of the program. While this is certainly ``cleaner'' to do if
possible, it is not mandatory.}.

Most of this program's action takes place in the tracking function.
Because absolute position tracking is implemented, the function
{\tt clip} is used
to limit the maximum displacement that can occur during any particular
cycle (\cref{5}).

The first thing {\tt trackFxn()} does is use {\tt getMemByAddr()} to get a
pointer to the control memory structure. It then
obtains a pointer to the {\tt MANIP} structure
using {\tt getManipByAddr()}, and uses this get a pointer to the
{\tt RCI_RBT} structure (\cref{7})
(section \ref{rcirbt}), which will be used to obtain and control sensor
inputs. 

The state variable {\tt tcb->tracking} indicates whether the
tracking function is active or not.
If tracking is active, then {\tt getInput()} is called to get the
raw $x$ and $y$ values from the sensor, which are then scaled by {\tt
tcb->gain}
and written into the {\tt xval} and {\tt yval} fields of the
control block (\cref{8}).
The maximum permitted
per-cycle displacement is computed from the velocity threshold and the RCCL
sample interval, and is used to clip the final $x$ and $y$ values which
are set in the transform.
If the {\tt tcb->compute} has been cleared, then input sensor
values are turned off with a call the {\tt turnInputOff()} (\cref{10}).
If the function is not tracking, then it waits for {\tt tcb->compute} to be
set by the planning level, then turns sensor inputs on with {\tt
turnInputOn()}.

The sensor I/O is encapsulated in separate routines. This
was done
mainly for show, but also because the sensor input code is a bit of a hack and
we didn't want to clutter up {\tt trackFxn()} with it. {\tt turnInputOn()}
instructs
the RCI robot interface to start reading analog-to-digital converter
channels 0 and 1 (\cref{12}),
or, if there are no ADC channels, announces that the sensor
input will be faked (\cref{13}).
{\tt turnInputOff()} closes the channels (\cref{14}). Finally,
{\tt getInput()} either reads back the ADC values from the {\tt HOW} structure
(\cref{15}) or fakes them with an ellipse pattern (\cref{16}).

The velocity limiting algorithm used in this example is kept very crude
for simplicity. Of course, more elaborate smoothing techniques, or even
general control laws, can be used as required by the application.

The original version of RCCL contained an example program in which a
linear potentiometer was attached to the robot tool tip and
was used to do depth tracking along surfaces. To accomplish
this, a very simple tracking function
{\tt fingerFxn()} was defined which adjusted the $z$ coordinate of
its transform using the following code:
\begin{verbatim}
        fingerFxn (tr, arg, mnp)
        TRSF *tr;
        int arg;
        MANIP *mnp;
        {
                HOW *how = mnp->how;

                tr->p.z + 100.0 * how->adcInput[CHAN] * GAIN;
        }
\end{verbatim}

If the sensor inputs are very slow or the corresponding computations are
very lengthy, one has the choice of either spreading the tracking
computation over several control cycles, or doing the tracking from the
planning level. Planning level tracking is tricky
mainly because the planning level does not have the
real-time priority of the control level (and can even be suspended for short
periods of time by UNIX). However, the reader might find one
of the following paradigms interesting:

\begin{list}{}{}
\item[1.]Make the tracking transform variable and set it from the planning
level at some reasonable rate. Presumably the changes
are not large.
\begin{verbatim}
        TRSF change;
        TRSF tmp;

        ...

        setTransVarb (track);
        readTrans (track, &tmp);        /* initialize */

        ...

        setTime (mnp, F_DEFAULT, F_UNDEF);
        move (mnp, pt);                 /* semi-permanent motion */
        while (!quit)
         { getSensorInput (&change);
           multTrsf (&tmp, &tmp, &change);
           writeTrans (track, &tmp);
           delay (REASONABLE_INTERVAL);
         }
\end{verbatim}

\item[2.] A more reasonable method than the one above would be to issue many
short motion requests from the planning level. This is preferable because then
the trajectory generator will interpolate and provide smoothing
between successive data points.
\begin{verbatim}
        setTransConst (track);

        ...

        setMotionQueueSize (mnp, 1);
        while (!quit)
         { getSensorInput (&change);
           multTrsf (track, track, &change);
           move (mnp, pt);
         }
\end{verbatim}

\item[]{\tt track} is set constant since each move request will then make
a private copy of it.
Setting the motion queue size to 1 prevents queue
saturation and ensures that each call to {\tt move()}
will block until the requested motion is being executed.
One problem with this method is that the first few cycles of any
motion incur a large amount of overhead (to compute the drive and
transitioning parameters). This effect will be greatly reduced if the
path is naturally smooth enough that the motion transition times can be set to
zero. A small fixed delay
within the loop would also help reduce the frequency of the motions.
\end{list}

\subsection{Guarded Motions}

The next example uses a monitor function to implement
guarded motions. The basic guarded motion paradigm
is very simple: a monitor function waits until
some particular condition is raised, and then cancels the motion. The planning
level can examine the {\it stop code} associated with the motion to see
whether
or not the cancellation actually occurred\footnote{This information could also be returned by the application
using shared memory, but motion
stop codes are explicitly incorporated into RCCL, so they may as well
be used. Stop codes exist mainly to make it possible
to (in the future) implement a feature where different {\it reflex} motions
are executed
depending on the stop code returned by the previous motion.}.

When doing guarded motions, it is often desirable to be able to determine
the value of a particular transform (within the context of a
specified position equation) at the instant of contact (or whatever
condition is being tested for). This computation has
to be done by the trajectory generator, because an unacceptable length of time
may elapse between when the guarded motion ends and when the planning task
learns of it and tries to record the appropriate data. Assume that we want to
know the value of a transform {\tt t0} in the context of some position
equation {\tt p0}. What we want to be able to do is
ask the trajectory generator to call {\tt solveTrans()} for {\tt t0}, using
position {\tt p0}, at the end of a particular motion. The function
\begin{verbatim}
        updateTrans (mnp, tr, p0, t0, tx)
          MANIP *mnp;
          TRSF_PTR tr, t0, tx;
          POS_PTR p0;
\end{verbatim}
will do just this. The evaluation is
done at the {\it end\/} of the next requested motion
on the manipulator {\tt mnp}. The last four
arguments to {\tt updateTrans()} are equivalent to the four arguments
of {\tt solveTrans()}. Usually, the position {\tt p0} will contain
a {\bf T6} transform, which we will want to be instantiated with the current
value of the manipulator's {\tt t6} field. The corresponding call looks
like this:
\begin{verbatim}
        updateTrans (mnp, tr, p0, t0, mnp->t6);
\end{verbatim}

All of the transform arguments to {\tt updateTrans()} must be allocated
using the {\tt allocTrans()} routines.

The sample program below simulates a situation
in which the manipulator supposedly has a contact sensor
with which it tries to find the locations of two points on a planar surface.
When each motion finishes, the current manipulator position at the
contact point is remembered and used afterwards
(if both motions achieved contact) to trace out a line along the surface
between the two points.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/guarded.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/guardedCtrl.560.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560
and lives at {\tt guarded.560.c} and {\tt guardedCtrl.560.c}
in {\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 is contained
in {\tt guarded.260.c} and {\tt guardedCtrl.260.c}. If the program discovers
that the system does not have right sensor inputs, then the monitor function
just punts and aborts the guarded motions at different points along their
trajectories.

}
\medskip

The program is a bit verbose, but fairly straightforward.
To illustrate a different way of specifying motion targets,
it only defines one position
equation {\tt p0} (\cref{2}):
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf TARGET}
\]
{\bf E} is the usual end-effector transform and {\bf TARGET} is a transform
which is set to a specific target location for each motion.
This can be done since all motion requests are to fixed
locations. Some transforms can then be declared as regular variables, since
they will be used only within the planning task (\cref{1}).
The transforms pointed to by {\tt contact} are
allocated using {\tt allocTrans()} since they will
be used as arguments to {\tt updateTrans()}. All of the transform values are
instantiated starting at (\cref{3}), after the robot has been moved to its
initial position.
{\bf E} is set to the canonical end-effector values. The locations in
{\tt start} describe the points at which the guarded motions will
begin; they
are defined simply by a translational offset from
the initial value of {\bf TARGET} in the position {\tt p0} (\cref{4}).

The touch sensor input is assumed to come from channel 0
of the analog-to-digital converters supplied by
the RCI robot/sensor interface. If ADC channels are available,
the program enables this channel at (\cref{5}).
It will be read by the monitor function.

The guarded motions are set up by the loop at (\cref{6}). Moving the
manipulator to a particular location is done by setting {\bf TARGET} to the
desired location and then queueing a move request to {\tt p0}. The first move
is to the start position. The next move is the guarded motion, which
involves setting up a few things. Cartesian interpolation is specified.
A slow velocity is set in anticipation of contact
(\cref{7}). An explicit
motion ID is requested, using
{\tt setMotionFlag()} (\cref{8}); this is so that the motion ID will be known
in advance and can therefore be passed as an argument to the monitor function
{\tt touchFxn()}, which is established with a call to {\tt runMotionFxn()}.
To store the location at the end of the guarded motion,
{\tt updateTrans()} asks the trajectory
generator to solve {\tt p0} for {\bf TARGET} at the end
of the motion and place the result in one of the transforms pointed to be
{\tt contact} (\cref{9}). Finally, the guarded motion itself
is implemented by requesting a move to a target offset from {\tt start}
by 200.0 millimeters along the $z$ axis (\cref{10}).
Since the guarded motion presumably makes contact with a hard surface,
we wish to follow it with a quick ``jump'' back up the $z$
axis by a small amount. This is accomplished using a relative
motion, created with another $z$ offset applied to the position {\tt
mnp->last} (\cref{11}).
The transition time is set to 0 so that the
manipulator will pull away from the surface immediately; otherwise,
the deceleration associated with the transition would carry the robot farther
into the surface. A transition time of 0 is tolerable since the
approach velocity is low.
After the pullback motion, the robot switches to joint mode and
moves back to the starting point (\cref{12}). The program itself uses
a motion ID to wait for the guarded move to complete (\cref{13}), and
then examines the stop code (which was set by the monitor function) to
determine if contact was actually achieved (\cref{14}).
A stop code of 1
indicates contact; otherwise, the system will have set the stop code
to {\tt ON_NORMAL} when the nominal endpoint was reached.

If both guarded moves make contact with the surface, the program
has the robot draw a line, at normal velocity,
5.0 millimeters above and between the two contact positions
(\cref{15}). The robot is
moved to the first start point, the first contact point, the second
contact point, and back to the first start point.
The 5.0 millimeter vertical displacement is achieved by
simply adding an offset to the
contact target positions (\cref{16}).

The monitor function {\tt touchFxn()} is quite simple. It begins by
fetching a pointer to the robot's {\tt HOW} structure
(\cref{17}; see section \ref{how})
from which it reads ADC channel 0. The motion is
canceled if this channel displays a value greater than {\tt THRESHOLD}
(\cref{18}).
If the system does not have any ADC channels, the monitor function fakes it by
canceling the motion when it is either 20\% or 60\% done (\cref{19}).

\section{Teaching Positions}

\subsection{The Teach Routine}
%\index {teach routine}{}

RCCL provides a teach routine, {\tt rcclTeach()}, which
allows a user to move the arm around using a combination of keyboard and teach
pendant commands:
\begin{verbatim}
        rcclTeach (mnp, prompt, tool)
          MANIP *mnp;
          char *prompt;
          TRSF *tool;
\end{verbatim}
{\tt rcclTeach()} assumes that the trajectory generator is currently active ({\it
i.e.}, {\tt rcclStart()} has been called.
When it returns (at the command of the operator), the
robot's new position can be read from the appropriate fields in
the {\tt mnp} structure (such as {\tt t6}, {\tt here}, {\tt
lastTC}, or {\tt j6};
the first three fields will all be equal).
If an error occurs
such that the teach routine was unable to successfully complete, it returns
-1 and an appropriate error code is placed on the
error stack (section \ref{errorStack}).
The argument {\tt prompt} defines the prompt used for keyboard input.
{\tt tool} is an optional argument that specifies a transform
applied to the T6 frame to define a TOOL frame.
If {\tt tool} is given as {\tt NULL}, then the T6 frame is used as the
TOOL frame.

{\tt rcclTeach()} allows the operator to specify motions in three
principal coordinate systems:
{\it joint\/}, {\it tool\/}, and {\it world\/}.
These are completely analogous to
their counterparts in the Unimation language VAL.
In
{\it joint}
coordinates the robot is moved by specifying individual joint angles. In
{\it tool\/}
coordinates the robot is moved by specifying Cartesian translations and
rotations relative to the TOOL frame.
In
{\it world}
coordinates, the robot is moved by specifying Cartesian
translations relative to the manipulator base frame and
rotations relative to the manipulator base frame translated so that
its origin is coincident with that of the TOOL frame.

\subsection{Keyboard Commands}
%\index {keyboard teach commands}{}

The keyboard commands are implemented using
the C-tree matcher (see section \ref{jmove}):
the terminal
displays a prompt and the user enters commands.
Typing {\tt ?} gives
either
a menu of valid commands or instructions as to what should be entered next.
Hitting {\tt <space>} automatically matches what the user has typed against
the offered commands and expands the input line accordingly.

\begin{desclist}
\item[{\tt return} --]
Causes {\tt rcclTeach()} to return with a value of 0.

\item[{\tt record} --]
Causes {\tt rcclTeach()} to return with a value of 1. This can be useful
when the teach routine is executing in a loop; a return value of 1 can
be used to signal that we want to record the current point (and
perhaps call {\tt rcclTeach()} again to record more points).

\item[{\tt stop} --]
Stops the arm ``dead in its tracks''.

\item[{\tt show position} --]
Prints the manipulator's current position in both Cartesian and
joint coordinates.

\item[{\tt show tool} --]
Prints the specified value of the {\it tool} transform.

\item[{\tt show t6*tool} --]
Prints the location of the TOOL frame relative to the
manipulator base frame.

\item[{\tt world <dof> by <value>} --]
Moves the indicated degree of freedom by a certain value
in {\it world\/} coordinates.
Translations are specified by a {\tt <dof>} of
{\tt x}, {\tt y}, or {\tt z}, and a {\tt value} in millimeters.
Rotations are specified by a {\tt <dof>} of
{\tt rx}, {\tt ry}, or {\tt rz}, and a {\tt value} in degrees.

\item[{\tt tool <dof> by <value>} --]
Moves the indicated degree of freedom by a certain value
in {\it tool\/} coordinates.
Values are specified the same way as for world coordinates.

\item[{\tt joint <num> by <value>} --]
Moves the indicated joint {\bf by}
a specified value (degrees for rotational joints
and millimeters for prismatic joints).

\item[{\tt joint <num> to <value>} --]
Moves the indicated joint {\bf to}
a specified value (degrees for rotational joints
and millimeters for prismatic joints).

\item[{\tt goto joints} --]
Moves all the joints of the robot to a specified set of values.

\item[{\tt set speed} --]
Sets a scale value for the motion speed (the default value is 1.0).
The default velocities used by {\tt rcclTeach()} are the current
values set by {\tt setCartVel()} and {\tt setJointVel()}.
Setting the speed within the teach routine will not change the speed
settings in the program when the routine returns.

\item[{\tt set translation <x> <y> <z>} --]
Moves the manipulator so that the $x$, $y$, and $z$
translational coordinates
of the TOOL frame (relative to the manipulator
base frame) are set to a particular value
(in millimeters).

\item[{\tt set rotation euler <phi> <the> <psi>} --]
Moves the manipulator so that the rotational component of
the TOOL frame (relative to the manipulator
base frame) is described by a particular set of Euler angles
(degrees).

\item[{\tt set rotation rpy <roll> <pitch> <yaw>} --]
Moves the manipulator so that the rotational component of
the TOOL frame (relative to the manipulator
base frame) is described by a particular set of roll-pitch-yaw angles
(degrees).

\item[{\tt set tool <x> <y> <z> <roll> <pitch> <yaw>} --]
Explicitly sets the {\it tool} transform to the specified translational
values (millimeters) and roll-pitch-yaw angles (degrees).

\item[{\tt set integration [on | off]} --]
Enables or disables servo level integration; when integration is
enabled, joint servo errors are reduced to zero, but the manipulator
may become more ``frisky''. This command is (at the moment) specific
to Unimation/PUMA robots.

\item[{\tt hand <value>} --]
Sets the manipulator hand to a particular value (for open/close grippers,
1 usually means ``open'' and 0 usually means ``closed'').
\end{desclist}

\subsection{Pendant Commands}
%\index {teach pendant}{}

The commands from the (PUMA) teach pendant are now described.

\begin{list}{}{}
\item[$\bullet$]
The buttons {\it joint\/}, {\it tool\/}, or {\it world\/} set the default
coordinate
system for subsequent teach pendant commands.

\item[$\bullet$]
The button {\it free} places the manipulator into a zero-gravity mode,
with all
joints limp and their gravity loadings compensated for; the operator
is then free to move the arm around into any position desired.

\begin{quote}
\bf WARNING: check to
make sure this command is working properly; if it isn't the robot
might fall.
\end{quote}

\item[$\bullet$]
The switch on the side of the pendant controls the speed of motion; it has
three settings: fast, medium, and slow.
The fastest setting is the momentary one.

\item[$\bullet$]
The {\it tick} button enables ``ultra-slow'' pendant motions; this is
for fine position adjustment.

\item[$\bullet$]
The {\it clamp1} and {\it clamp2} buttons (usually) open and close the
gripper.

\item[$\bullet$]
The {\it REC} button causes the teach routine to return with a value of 1,
as though the keyboard command {\tt record} had been entered.
This is useful if the operator is teaching a whole set of points and does
not wish to return to the terminal.

\item[$\bullet$]
The joint control buttons (1 to 6) do different things depending on the mode.
In
{\it tool\/}
or
{\it world\/}
mode, they cause relative translations or displacements: the
first three buttons create $\pm$ motions along the $x$, $y$, and $z$ axes,
while
the last three buttons create $\pm$ rotations about
the $x$, $y$, and $z$ axes.
In
{\it joint}
mode, each button controls its corresponding joint. In
{\it free}
mode, the ($+$) side of each button ``locks'' its corresponding joint, so
that it is no longer limp, while the ($-$) side limps it again.
\end{list}

\subsection{Programming with the Teach Routine}

{\tt rcclTeach()} may be embedded inside applications and called whenever it
is desirable for an operator to move the arm around explicitly, either for
teaching new positions or just for the fun of it.

This is best illustrated by the following sample program, in which the teach
routine is used to record a sequence of positions (and hand settings), and
then ``play them back''.

\cverbatimfile{exampleCode/playback.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA robot
and lives at {\tt playback.c} in
{\filefont \$RCCL/demo.rccl}.

}
\medskip

Like the teach routine itself, this program uses the C-tree matcher.
A brief description of the C-tree matcher is given for the
example program in section (\ref{jmove}). Detailed information is provided by
the document {\tt CtreeMatch.doc} in {\filefont \$RCCL/doc}.

Definitions relevant to the C-tree matcher are included with the file
{\tt <ctree.h>} (\cref{1}). The taught positions are stored (in joint
coordinates) in the array {\tt points}, with the hand position settings
in the array {\tt handSet} (\cref{2}). Binary (open/close)
hand settings are assumed.

The call to {\tt tree_match_parse()} (\cref{3}) sets up the keyboard commands
that will be accepted by the Ctree-matcher. These commands
include
\begin{verbatim}
        quit
        set speed <value>
        show
        record
        playback
\end{verbatim}
Since the program does not set the option {\tt RCCL_ERROR_EXIT}, it
checks the routines {\tt rcclCreate()} and {\tt rcclStart()} explicitly for
error conditions; if errors are detected, the error information is printed
by {\tt printErrors()} (see section \ref{errorStack})
and the program exits (\cref{4}).

Commands are read in by the routine {\tt tree_match()} (\cref{5}),
in a loop, until the command {\tt quit} is entered.

The {\tt set speed} command (\cref{6})
calls {\tt setSpeed()} to set
the speed with which the program will
play back the recorded positions

The {\tt show} command (\cref{7})
prints out the current playback speed setting
and the number of playback points which are currently stored.

The {\tt record} command (\cref{8})
allows the operator to teach a set of points, which is done by calling
{\tt rcclTeach()} in a loop. The teach routine will return
each time the operator enters the {\tt record} command from the keyboard,
hits the {\tt REC} button on the teach pendant, or enters the
command {\tt return}. In the first two cases, the routine will return
1, which the program uses as a signal to record the current point
and call the teach routine again (\cref{9}).
The joint values of each recorded point are read from the
{\tt j6} field of the {\tt MANIP} structure.
The hand settings are read from the {\tt handPos} field of
the {\tt MANIP} structure.
If {\tt rcclTeach()} returns
a negative value, this indicates an error has occurred, and the program
prints out information about the error and exits (\cref{10}).

The {\tt playback} command (\cref{11}) plays back all points which are
currently recorded. This is done simply by calling {\tt movej()}
for each recorded point, and, if the hand position for that point
is different from the current hand position, waiting for the move to finish
and setting the new hand value appropriately.

\section{Logging Data}\label{logging}

In RCCL programs, it is often necessary to log data created or read in
at the control level. For instance, an experimenter working with
force control algorithms might want to record, for every control
cycle, the force sensor values and the current position of the robot.

The convenient thing to do in these cases would be to write the
data directly to a file. Unfortunately, one cannot access files
directly from the RCCL control level. Instead, the data must be
written into an intermediate buffer (implemented using shared
memory) which the planning level flushes to a file when it gets
full. This is not very difficult to do, and once written, the
actual code can be applied to most similar situations.
We have written a short program
demonstrating this.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/logger.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/loggerCtrl.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for all systems,
and lives at {logger.c} and {\tt loggerCtrl.c}
in {\filefont \$RCCL/demo.rccl}.

}
\medskip

All this program does is start the trajectory
generator, and then turn on a monitor function which logs the current
joint setpoints. The monitor writes the log data into a buffer,
and when the buffer is full, it sends a signal to the planning task, which
then writes the contents out to the log file. A double buffer system
is used, as described in section \ref{doublebuf}. The buffer itself
is implemented using a shared memory object defined as follows:
\begin{verbatim}
        #define BUFSIZE 4096

        typedef struct {
                  int bufnum;
                  char buf[2][BUFSIZE];
                  int count[2];
        } DATABUF;
\end{verbatim}
{\tt bufnum} is the index of the buffer currently being
written to by the monitor function. The double buffer
itself is the {\tt buf} element.  The field {\tt count}
describes how much data is in each of the two buffers.
In general, the size of the buffer ({\tt BUFSIZE} in this case)
should be selected so that UNIX has time to catch the ``buffer full''
signal and write all the data out to a file before the second buffer
fills up. What this number is depends on how much data is being
logged, the control sample rate, and the UNIX system itself. To be
safe, one should probably make the buffer big enough to
accommodate 1 second worth of data, and also should try to avoid
writing
so much as to cause the system to choke. Logging about 10 Kbytes
per second should certainly cause no difficulties, and more
is certainly possible.

When the planning task receives the ``buffer full'' signal,
it calls the signal handler {\tt logHandler()} (\cref{1}),
which in turn calls {\tt writeLogFile()} (\cref{2}) to write out the buffer.
The second argument to {\tt writeLogFile()} is the number of the
buffer which should be written. This is set to the buffer opposite
from {\tt bufnum}, that is, the buffer not currently being written to
by the control level.

We will now look at the program itself. {\tt SIGUSR1} is used to indicate
``buffer full''; {\tt logHandler()} is bound to this signal at (\cref{3}).
The log file is opened at (\cref{4}) and the data buffer structure is
allocated at (\cref{5}). {\tt bufnum} is set to {\tt UNDEF} to
tell the monitor function it has to initialize the data structure.
Once everything has been set up, the monitor function is started at
(\cref{6}), the program naps for 10 seconds, and then the monitor
function is deleted (\cref{7}).
We delay for a control cycle to make sure that we have caught the last
``buffer full'' signal. Then, the rest of the buffer that was being filled
at the time the monitor was turned off is written out (\cref{8}).

The logging function itself is fairly straightforward. It does some
initialization if the {\tt bufnum} field is set to {\tt UNDEF} (\cref{9}).
It then sets the variable {\tt str} to the point in the buffer
where it left off the last time (\cref{10}), and prints information
there using {\tt sprintf()}.
The buffer counter
is increased by the size of the string that was created (\cref{11}), and
the string length is also used to decide if there is enough buffer
space for another
cycle (\cref{12})\footnote{This particular code might break if the string is unexpectedly longer during
the next cycle; the necessary checks were omitted from this
example for simplicity. }.
If there is not, we switch buffers and signal the planning task (\cref{13}).

If space is very tight, one does not have to use
an ASCII data format as we have done here. Binary formats are several times
more compact (and usually much faster), although they are less portable and
generally require a program interface to read.

A programmer doing a lot of work involving data logging
would probably want to encapsulate the logging code
presented here into a small set of routines.

\newpage
\done
