\input rccluginit.latex

\chapter{Controlling a Robot}

This section describes the primitives available for doing actual 
robot control.

\section{The Create and Delete Primitives}\label{create}

When an RCCL program wishes to control a robot, it must first create
a {\tt MANIP} structure for that robot. This is done using the call
\begin{verbatim}
        MANIP_PTR rcclCreate (robotName, cpuMask)
          char *robotName;
          int cpuMask;
\end{verbatim}
which looks up the named robot in the RCI system tables (the robot
should be one configured into RCI), 
allocates the necessary control data structures
for it (including the {\tt MANIP} structure), and assigns
this robot to a trajectory task on one of the CPUs specified by
the bitmask {\tt cpuMask}, creating that task if necessary.
The last argument is important only if
the RCCL/RCI site does in fact support multiple CPUs. 
In either case, it is
usually set to 0, which tells the system to automatically
select a CPU from those which are available.

The {\tt MANIP} pointer returned by {\tt rcclCreate()} is the handle
used for all subsequent control of that robot. When the robot is
no longer needed, its {\tt MANIP} structure
may be removed with the call {\tt rcclDelete(mnp)}.

\section{The MANIP structure}

Information about the robot can be obtained from the {\tt MANIP}
structure.
The fields which normally concern the programmer look like this:
\begin{verbatim}
        typedef struct {

           /* Cartesian level information */

           TRSF *t6;
           TRSF *t6o;
           TRSF *here;
           TRSF *tool;
           TRSF *rest;
           TRSF *lastTC;

           POS *park;
           POS *last;

           /* Joint level information */

           JNTS *j6;
           JNTS *j6o;
           JNTS *jvel;
           int handPos;

           /* Robot information */

           RCI_RBT *rbt;
           HOW *how;
           JLS *jls;
           KYN *kyn;
           void *var;

        } MANIP, *MANIP_PTR;
\end{verbatim}
{\tt t6} contains the output value of the manipulator
{\bf T6} transform computed by the trajectory generator
during the most recent control cycle.

{\tt t6o} contains the observed values of
{\bf T6}, as read back from the robot during the last control
cycle. For computational efficiency,
this field is maintained only if the
mode {\tt T6O_EVAL} is selected for the manipulator
(see section \ref{modes}).

{\tt here} contains the value of the {\tt t6} field
at the point where the
last motion ended (as in ``at the end of the last motion,
{\bf T6} was {\it here}''). See section \ref{motions} for a
discussion of just when a motion ``ends''.

{\tt tool} contains the value of the current target position's
{\bf TOOL} component, as computed by the trajectory generator during 
the most recent control cycle.

{\tt rest} is a fixed read-only transform corresponding to the 
park position of the manipulator. The actual definition of this is
a little up in the air; at the moment, the system looks up the
values of the robot's ``park angles'' in the its {\tt JLS} structure
(see section \ref{rcirbt}) and computes the corresponding 
{\bf T6} transform value.

{\tt lastTC} is the target
value of {\bf T6} towards which the manipulator was
heading at the end of the last motion (this differs from {\tt here}
because of transitioning between path segments; {\bf T6} usually comes
close to {\tt lastTC} but does not achieve it; see section \ref{motions}).

{\tt park} is a built-in position equation corresponding to the
manipulator's park position:
\[
{\bf T6} \;=\; {\bf REST}
\]
where {\bf REST} is the transform contained in the {\tt rest} field.
This allows moves to the park position to be simply specified with
the following command:
\begin{verbatim}
        move (mnp, mnp->park);
\end{verbatim}

{\tt last} is a built-in position equation which describes the last
target the manipulator was moving to:
\[
{\bf T6} \;=\; {\bf LASTTC}
\]
where {\bf LASTTC} is the transform contained in the {\tt lastTC} field.
It is particularly useful in conjunction with the {\tt distance()}
primitive to create relative motions (see below).

{\tt j6} contains the output manipulator joint values
computed by the trajectory generator during the most recent control cycle
(if you apply forward kinematics to these, you will get the values
in the {\tt t6} field).

{\tt j6o} contains the actual observed values of the joint
values, read back from the robot during the last control cycle.
For computational efficiency, this field is maintained only if the mode
{\tt T6O_EVAL} is selected for the manipulator
(see section \ref{modes}).

{\tt jvel} contains the current output (computed) joint velocities.

The fields {\tt rbt}, {\tt how}, {\tt jls}, {\tt kyn}, and {\tt var}
point to memory areas describing the low level RCI interface to the
robot. In particular, they describe parameters and terms relating
to a particular robot's physical characteristics. These fields
are discussed in detail in section \ref{rcirbt}.

Other 
information can be obtained from the {\tt MANIP} structure 
using special macros. The macro {\tt MANIP_CPU(mnp)} returns the
CPU number of the manipulator's trajectory task.
The macro {\tt MANIP_TASK(mnp)} returns the RCI task descriptor for the
manipulator's trajectory task (you won't really need this,
but check the {\it RCI User's Guide} if you are curious).

\section{Running the Trajectory Generator}

It is necessary to turn on the trajectory generator task before the robots
can actually be controlled. The routine to do this
is 
\begin{verbatim}
        rcclStart()
\end{verbatim}
The trajectory generator will then begin executing at a regular sample
interval, the default value of which is a parameter 
in the {\tt .rciparams} file (see section \ref{params}).
The sample interval can be read back or changed from within
the RCCL program using the primitives
\begin{verbatim}
        rcclGetInterval()

        rcclSetInterval(interval)
\end{verbatim}
It is illegal to call {\tt rcclSetInterval()} 
(and {\tt rcclCreate()} as well) while the trajectory
generator is running.

Turning the trajectory generator off
is done with the routine 
\begin{verbatim}
        rcclRelease(powerOff)
\end{verbatim}
The parameter {\tt powerOff} is a boolean which, if true, causes the
robot arm power to be turned off as well (unless the option
{\tt RCCL_LEAVE_POWER_ON} is set).

To determine if the trajectory generator is running, the routine
\begin{verbatim}
        rcclActive()
\end{verbatim}
can be called; this returns true if the trajectory generator is running.

\section{Specifying Motions}
%\index {motion description}{}

\subsection{The Basic Motion Mechanism}\label{motions}

The basic primitives to request manipulator motions are:
\begin{verbatim}
        move (mnp, pos)
          MANIP *mnp;
          POS_PTR pos;

        movej (mnp, jnts)
          MANIP *mnp;
          JNTS_PTR jnts;
\end{verbatim}
{\tt move()} tells the trajectory generator to move the manipulator {\tt mnp}
to the target specified by {\tt pos}, which must be
a position equation returned by
{\tt makePosition()}.
{\tt movej()} tells the system to move the manipulator
to a target specified by a set of joint values.

These primitives do not wait until the manipulator has reached the specified
target point. Instead, they simply queue up a {\it motion request}
and return. The trajectory
generator task services the motion requests in FIFO fashion 
(figure \ref{queue}). This decouples the activity of the planning level from
the control level, but also means that explicit synchronization primitives
must be provided. The simplest of these is the {\tt waitForCompleted()}
primitive described in the first example program; others will be presented
later. 

The planning level can set 
parameters controlling various motion characteristics, such as the desired
velocity or the interpolation mode; these will be discussed
below. Usually these parameters are lumped
in and queued with the motion requests themselves, so they take effect
only with the next motion request.

\begin{figure}[tbh]
%\rect 5truein by 3.5truein
\boxedsizedfig{5truein}{3.5truein}%
{\psfig{figure=figs/queue.idr}}%
{Motion queue and shared memory block
linking the planning and control levels internally. The queue is
for storing motion request packets, while the shared memory block
is for information that is transferred immediately.}%
\label{queue}
\end{figure}

Each motion request serviced by the trajectory generator
corresponds to a single {\it motion path segment}. When finishing one
path segment and starting another, it is generally necessary to provide
a smooth transition from the first path segment into the second.
The purpose of the transition is to prevent the manipulator 
from undergoing large impulses in acceleration or higher
derivatives. This is illustrated (with a one-dimensional example)
in figure \ref{trans}, which shows a two path segment trajectory
in which the manipulator is asked to move first from $A$ to
$B$, and then from $B$ to $C$. The idealized trajectory is
a dotted line, and the real trajectory is a solid line. At the
start of each path segment there is a transition region
which is symmetric about the nominal (or {\it official\/})
starting time for the
path segment. The time on each ``side'' of this transition is
$\tau$ control cycles, which makes the total transition time $2 \tau$
cycles; this is marked for the transition between path segments
$\overline{AB}$ and $\overline{BC}$. Outside of the transition zone, the
manipulator ``coasts'' at a constant velocity.
The total
time allocated for the path segment (from the midpoint of one transition to
the midpoint of the next) is $\sigma$ motion counts.
The trajectory generator normally
computes the values of $\tau$ and $\sigma$ for each
motion ``on the fly'', using the velocity and acceleration limits
currently specified for the manipulator.

\begin{figure}[tbh]
%\rect 6truein by 3.5truein%
\boxedsizedfig{6truein}{3.5truein}%
{\psfig{figure=figs/trans.eps}}%
{Manipulator trajectories are created by
blending together individual path segment, as illustrated
here in one dimension.}%
\label{trans}%
\end{figure}


The existence of transition regions creates some ambiguity about when one
motion segment ends and another begins. By default, RCCL defines the
{\it official} time at which this happens
to be the {\bf midpoint} of the transition region\footnote{Except for
``stop'' motions, described in the next section, which are considered
to end at the beginning of the transition to the next motion.} %
(see figure \ref{startstop}).
This is when the {\tt here} transform and various
motion status flags are updated.
On the other hand, the actual computations associated with a path
segment (including the execution of monitor and transform functions,
which will be discussed later) are started at the {\bf beginning} of
the transition interval (and the corresponding
computations for the previous path segment
are halted). This is an artifact of the way RCCL computes
trajectories. If this slight inconsistency causes trouble, then
the official motion change over time can be set to the
beginning of the transition interval as well (see section
\ref{modes}).

\begin{figure}[tbh]
%\rect 6truein by 3.5truein%
\boxedsizedfig{6truein}{3.5truein}%
{\psfig{figure=figs/startstop.eps}}%
{The ``switchover'' point for motions
is generally taken to be the midpoint of the transition region.}%
\label{startstop}%
\end{figure}


\subsection{Stopping at Target Points}

Because of the motion transitions, a manipulator will generally not
pass directly through its target point unless it is made to stop
there; instead, it will tend to ``undercut'' the target point.
This is the difference between the {\tt lastTC} and {\tt here}
fields (in the {\tt MANIP} structure) which are updated
at the end of each motion: {\tt lastTC} corresponds to the ideal
target position ($B$, in figure \ref{startstop}), while {\tt here}
corresponds to the point actually reached by {\bf T6}.

In RCCL, stopping a robot is accomplished by essentially moving to the
same target position {\it twice}. The second ``motion'' ensures that
the manipulator will actually come to rest at the target, as
illustrated in figure \ref{stopfig},
which shows the manipulator stopping at $B$ for a short time.
This is the same paradigm as that described
in Richard Paul's book [Paul 1981]. 

\begin{figure}[tbh]
%\rect 6truein by 3.5truein%
\boxedsizedfig{6truein}{3.5truein}%
{\psfig{figure=figs/stopfig.eps}}%
{Stopping at a point is done using two path
segments which have the same target.}%
\label{stopfig}%
\end{figure}


Motion requests to stop the manipulator are invoked by the {\tt stop()}
primitive, 
\begin{verbatim}
        stop (mnp, time)
          MANIP *mnp;
          float time;
\end{verbatim}
which stops the manipulator at the last target position for {\tt time}
milliseconds. 

Note that stopping at a target position does not necessarily
mean the manipulator will physically come to rest, since 
if the target itself is moving in time, then the manipulator will continue
to track it. One the other hand, when all the motion requests
for a manipulator have been executed, the trajectory generator
will normally bring it to an absolute stop at wherever it happens
to be. In other words, when a program waits for all requested motions to
finish with the macro
\begin{verbatim}
        waitForCompleted (mnp);
\end{verbatim}
the manipulator will usually be brought to an absolute
stop.
This an intuitively reasonable thing to do: ``no motion has been specified,
so don't do anything''. On the other hand, it is sometimes desirable
in these circumstances to have the robot continue to track the last
target position\footnote{This is what the original version of RCCL did.}.
This behavior can be selected by setting {\tt TRACKING_MODE} 
(section \ref{modes}) for the manipulator in question; if this is done,
{\tt waitForCompleted()} will still block until all explicit motions
have finished, but the manipulator might not be brought to an absolute
stop.

Although similar in effect to issuing another move to the
last target position,
{\it stop} requests are slightly different from {\it move} requests
in several ways.
First, 
they ignore the setting of certain motion parameters.
In particular, the effect of calls to {\tt setMod()}, {\tt setConf()},
{\tt distance()}, and {\tt setDistance()}
(see below) is held over until the next {\it move} request.
Second, the trajectory generator treats stop requests differently in that
no ``drive'' computation is performed for them.
Last, a stop motion is considered to be officially ``over'' when
the transition to the next motion segment begins (instead of at the transition
region midpoint). 

\section{Setting Motion Parameters}

\subsection{Interpolation Mode}

As described in section \ref{motiontarget}, 
RCCL implements two ways of moving between target positions:
joint interpolation, also known as {\it joint mode}, and
%\index {joint mode}{}
Cartesian interpolation, or {\it Cartesian mode}.
%\index {Cartesian mode}{}
Because a joint interpolated path always exists between two
well defined end points, joint mode is usually preferred for
large motions. Cartesian mode, on the other hand, generates trajectories
which are easy to visualize and which are often suited to particular
types of tasks.

Motions specified with {\tt move()} are performed in either
joint or Cartesian mode, depending on which is currently selected.
Selection of the interpolation mode is done on a per-manipulator
basis with the primitive
\begin{verbatim}
        setMod (mnp, c)
          MANIP *mnp;
          char c;
\end{verbatim}
{\tt c} should be set equal to {\tt 'j'} for joint mode and
{\tt 'c'} for Cartesian mode;
Joint mode is the default.
A change introduced by {\tt setMod()} 
takes effect with the next requested
motion, and remains in effect for all subsequent motions.
As an illustration of its usage, consider the following:
\begin{verbatim}
        MANIP *mnp;
        POS_PTR p, p1, p2;
        int i, m;

        p1 = makePosition(...);
        p2 = makePosition(...);

        for (move(mnp, p2), i = 0; i < 10; ++i)
         { if (i % 2 != 0)
            { m = 'c';
              p = p1;
            }
           else
            { m = 'j';
              p = p2;
            }
           setMod (mnp, m);
           move (mnp, p);
        }
\end{verbatim}
This will cause the arm to move from {\tt p2} to {\tt p1} ({\tt i} odd) in
{\it Cartesian}
mode and from {\tt p1} to {\tt p2} ({\tt i} even) in
{\it joint}
mode.
%\index {joint mode}{}
Of course, C experts will insist on coding the same thing like this:
\begin{verbatim}
        for (move(mnp, p2), i = 10; i--;)
         { setMod (mnp, (m = i % 2) ? 'c' : 'j');
           move (mnp, m ? p1 : p2);
        }
\end{verbatim}

To read back the mode setting for a manipulator, the routine
\begin{verbatim}
        getMod (mnp)
\end{verbatim}
may be used.
Since the mode setting has to propagate through the motion queue before
actually taking effect, the mode setting read back with {\tt getMod()} may not
be the one currently in use by the trajectory generator. The routine
\begin{verbatim}
        getActiveMod (mnp)
\end{verbatim}
returns the interpolation mode in effect for the current motion.

Motions specified with {\tt movej()} are always joint interpolated.

\subsection{Setting Velocities and Motion Times}

There are many ways to specify the desired
manipulator velocity. 

The simplest
way to change the robot speed is to call the
routine 
\begin{verbatim}
        setSpeed (mnp, scale)
          MANIP *mnp;
          float scale;
\end{verbatim}
which simply scales the base velocity up or down (the
default value for {\tt scale} is, predictably, 1.0). The same effect can
be achieved by setting the {\tt speed} parameter in the 
{\tt .rciparams} file (see section \ref{params}); this speed parameter
is combined with whatever the programmable speed scale happens to be.
The new speed setting will take effect with the next requested motion.
The speed setting can be read back using the routine {\tt getSpeed(mnp)}.

Setting a manipulator base velocity is slightly more complicated
since it depends on the interpolation mode: when doing the
straight line motion associated with Cartesian mode, one
wishes to specify velocities in terms of something like
mm.\ per sec.\ for translation and degrees per sec.\ for rotation.
When moving in joint interpolated mode, the velocities need to
be specified in units particular to the joints themselves.

For controlling the Cartesian mode velocity, the following
primitives are available:
\begin{verbatim}
        setCartVel (mnp, transVel, rotVel)
          MANIP *mnp;
          float transVel, rotVel;

        getCartVel (mnp, transVel, rotVel)
          MANIP *mnp;
          float *transVel, *rotVel;

        getDefaultCartVel (mnp, transVel, rotVel)
          MANIP *mnp;
          float *transVel, *rotVel;
\end{verbatim}
{\tt setCartVel()} sets the base Cartesian velocity to {\tt transVel}
mm.\ per second and {\tt rotVel} degrees per second. These represent
{\it limits}: the arm will accelerate to the lowest of the two bounds
and then maintain that speed.
For example, for a motion involving a 30 millimeter
translation and a 30 degree rotation, the call
\begin{verbatim}
        setCartVel (mnp, 30.0, 300.0);
\end{verbatim}
will result in a 1 second motion caused by the translation constraint,
overriding the
$1/10$ of a second time needed to perform the rotation.
If either of the arguments is
specified as {\tt F_UNDEF}, then it is left unchanged; if
either is specified as {\tt F_DEFAULT}, then it is set to the
system default value. {\tt getCartVel()} and
{\tt getDefaultCartVel()} allow the currently selected values
and the system default values to be read back.
There is no {\tt getActiveCartVel()} per se, since the base velocities
and scale factors are combined into a single net velocity before
being sent to the trajectory generator; for the
equivalent function, see section \ref{morevel}.

For controlling the velocity of joint interpolated motions, the following
primitives are available:
\begin{verbatim}
        setJointVel (mnp, jvel)
          MANIP *mnp;
          JNTS *jvel;

        getJointVel (mnp, jvel)
          MANIP *mnp;
          JNTS *jvel;

        getDefaultJointVel (mnp, jvel)
          MANIP *mnp;
          JNTS *jvel;

        setJvelScale (mnp, scale)
          MANIP *mnp;
          float scale;

        getJvelScale (mnp)
          MANIP *mnp;
\end{verbatim}
{\tt setJointVel()} sets the base velocities for each joint to
the values given by {\tt jvel}. These will be
millimeters per second for prismatic joints and degrees per second for
rotational joints. Again, these represent
{\it limits}: the arm will accelerate to the lowest of the bounds
and then maintain that speed. If any of the velocities
are specified as {\tt F_UNDEF}, then they are left unchanged; if
any are specified as {\tt F_DEFAULT}, then they are set to the
system default value. {\tt getJointVel()} and
{\tt getDefaultJointVel()} allow the currently selected values
and the system default values to be read back. The overall
joint velocities may be scaled using {\tt setJvelScale()} and
{\tt getJvelScale()}; the joint velocity scale factor acts
in addition to the scale factor specified with {\tt setSpeed()}.
There is no {\tt getActiveJointVel()} per se, since the base velocities
and scale factors are combined into a single net velocity before
being sent to the trajectory generator; for the
equivalent function, see section \ref{morevel}.

\subsection{Setting Acceleration}

The acceleration limits for a manipulator control the length of
the path segment transition times.
The higher the allowed accelerations, the shorter the transition
times.
Acceleration
limits can be specified with the following primitives:
\begin{verbatim}
        setAccelScale (mnp, scale)
          MANIP *mnp;
          float scale;

        float getAccelScale (mnp)
          MANIP *mnp;

        setCartAccel (mnp, transAcc, rotAcc)
          MANIP *mnp;
          float transAcc, rotAcc;

        getCartAccel (mnp, transAcc, rotAcc)
          MANIP *mnp;
          float *transAcc, *rotAcc;

        setJointAccel (mnp, jacc)
          MANIP *mnp;
          JNTS *jacc;

        getJointAccel (mnp, jacc)
          MANIP *mnp;
          JNTS *jacc;
\end{verbatim}
These routines are analogous to 
{\tt setSpeed()}, {\tt getSpeed()}, {\tt setCartVel()}, {\tt getCartVel()},
{\tt setJointVel()}, and {\tt getJointVel()}.
{\tt setAccelScale()} scales the acceleration limits for both
Cartesian and joint mode motions; the default scale value is 1.0.
{\tt getSpeed()} reads back the acceleration scale.
{\tt setCartAccel()} sets the acceleration limits for Cartesian motions
to {\tt transAcc} ${\rm mm} / {\rm sec}^2$ and {\tt rotAcc}
${\rm deg} / {\rm sec}^2$, while {\tt setJointAccel()} sets
the limits
for joint interpolated motions to the
values given by {\tt jacc} (${\rm mm} / {\rm sec}^2$ for
prismatic joints and ${\rm deg} / {\rm sec}^2$ for rotary joints).
Specifying any of the accelerations as {\tt F_UNDEF} causes the
corresponding value to be unchanged; specifying
{\tt F_DEFAULT} causes the system default value to be used.
The routines {\tt getCartAccel()} and {\tt getJointAccel()} allow
the current values to be read back.
Changes created by any of these routines take effect with the
next requested motion.

\subsection{More on Velocity and Acceleration Limits}\label{morevel}

Assume that $s$ is the speed scale factor controlled by
{\tt setSpeed()}, $s_g$ is the ``program global'' speed factor
set in the {\tt .rciparams} file (section \ref{params}),
$v_{bt}$ and $v_{br}$ are the ``base'' translational and rotational
velocities controlled by {\tt setCartVel()}, ${\bf v}_{bj}$
is the ``base'' joint velocity vector controlled with
{\tt setJointVel()}, and $s_j$ is the joint velocity scale controlled
by {\tt setJvelScale()}. These terms are combined into
``net'' velocities, described by $v_t$, $v_r$, and ${\bf v}_j$,
as follows:
\[
v_t \; = \; s \, s_g \, v_{bt}
\]
\[
v_r \; = \; s \, s_g \, v_{br}
\]
\[
{\bf v}_t \; = \; s \, s_g \, s_j \, {\bf v}_{bj}
\]
These net velocities are bundled into each motion request. The trajectory
generator uses them as required to determine an appropriate value of
$\sigma$ (section \ref{motions}) for the motion.

The values of $v_t$, $v_r$, and ${\bf v}_j$ which are in effect
for the current manipulator motion can be read back using the
routines 
\begin{verbatim}
        getActiveCartVel (mnp, transVel, rotVel)
          MANIP *mnp;
          float *transVel, *rotVel;

        getActiveJointVel (mnp, jvel)
          MANIP *mnp;
          JNTS *jvel;
\end{verbatim}

Acceleration limits are handled similarly. 
Let $\alpha$ be the acceleration scale controlled by
{\tt setAccelScale()},
$a_{bt}$ and $a_{br}$ be the ``base'' translational and rotational
accelerations controlled by {\tt setCartAccel()}, and 
${\bf a}_{bj}$
be the ``base'' joint acceleration vector controlled with
{\tt setJointAccel()}.
The ``net'' acceleration limits
$a_t$, $a_r$, and ${\bf a}_j$ are computed as follows:
\[
a_t \; = \; \alpha \, a_{bt}
\]
\[
a_r \; = \; \alpha \, a_{br}
\]
\[
{\bf a}_j \; = \; \alpha \, {\bf a}_{bj}
\]
These are bundled into each motion request and used by the trajectory
generator to estimate an appropriate transition time
$\tau$.
The values of $a_t$, $a_r$, and ${\bf a}_j$ in effect
for the current manipulator motion can be read back using the
routines 
\begin{verbatim}
        getActiveCartAccel (mnp, transAcc, rotAcc)
          MANIP *mnp;
          float *transAcc, *rotAcc;

        getActiveJointAccel (mnp, jacc)
          MANIP *mnp;
          JNTS *jacc;
\end{verbatim}

Finally, there is another routine, {\tt getActiveMotionCounts()},
which permits the program to read back the values of $\tau$ 
and $\sigma$ (the times for the incoming transition and motion duration,
respectively, in units of control cycles; see section \ref{motions}), which
are in effect for the current motion:
\begin{verbatim}
        getActiveMotionCounts (mnp, tau, sigma)
          MANIP *mnp;
          int *tau, *sigma;
\end{verbatim}

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

This is a simple program which repeatedly moves the robot
around the edges of a square ``box'' while
demonstrating the effect of changing the speed and acceleration
specifications.

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

}
\medskip

As with {\tt simple.560}, the program begins by including the
standard file {\tt <rccl.h>} (\cref{1}) and the manual example
file {\tt "manex.560.h"}, and declaring pointers for the various
transform and position structures (\cref{2}).

The arrays {\tt accelValues} and {\tt velValues} (\cref{3}) store
the different Cartesian acceleration and velocity values that 
will be used. Note the explicit use of the floating point quantity
{\tt F_DEFAULT}. The arrays {\tt accelDesc} and {\tt velDesc} contain
string information that the program will print as it executes.

As with the first example, the program sets 
the {\tt RCCL_ERROR_EXIT} option to enable
an automatic program abort if the RCCL routines detect an error condition
(\cref{4}) ({\it most\/} RCCL and RCI primitives respond to this
option; the reference manual should be consulted
to check whether or not a particular one does).
We repeat that while
this option does not have to be set, it can be
very convenient.
The name of the robot to be controlled is again taken to be
the system default robot (\cref{5}).

Several transformations are created (\cref{6}).
{\tt e} is a transform
from the robot ${\rm T6}$ frame to the tool tip; in this case (as with most
of the other examples) it is simply a translation along the $z$ axis
of the T6 frame. 
Transform {\tt b} locates the robot's initial starting position
in terms of a displacement from the manipulator base frame to the
tool frame.
It is used in the position equation {\tt start}, which is defined as
\[
{\bf T6} \;\; {\bf TOOL} \;=\; {\bf B}
\]
and is created with the call to {\tt makePosition()} at (\cref{7}).
Four separate transforms 
{\tt c[k]} are created to locate each of the four
corners of the box. Each corner is reached
by a translation in the $xz$ plane of the frame defined by {\tt b}
and is described by an equation of the form
\[
{\bf T6} \;\; {\bf TOOL} \;=\; {\bf B} \;\; {\bf C}(k).
\]
The corresponding position equations {\tt corners[k]}
are created at (\cref{8}).

Note that the transforms {\tt b} and {\tt e} are named but all the other
transforms and position equations are unnamed. The naming of the first
two transforms was done for illustrative purposes only; the names will
not actually be used in this program.
Note also that four separate transforms and position equations were created
to represent each of the box's corners. Again, this was mainly
done for illustrative purposes; for fixed targets, 
one has the option of creating
only one position equation and then moving to different points by changing
the values of its component transforms between move requests. Later
examples will illustrate this.

After the transforms and position equations have been allocated, the
program 
creates a {\tt MANIP} structure for the robot using
{\tt rcclCreate()} and turns on the trajectory generator
with {\tt rcclStart()} (\cref{9}). It should be mentioned
that while the examples in this manual
will tend to create their transform and
position objects before the first call to {\tt rcclStart()}, this is
not necessary; these can be created at any time,
whether the trajectory generator is running or not.

The first action is to move the robot to the starting 
position (\cref{10}). By
default, this will be done in joint mode, using the default speed
settings. The robot stops at the start position for 1 second before
proceeding, and the program waits for this pause to finish before
proceeding (\cref{11}).

The program next switches to Cartesian interpolation mode (using
{\tt setMod()} (\cref{12})) and goes into a loop 
(\cref{13}) which causes the
robot to trace out the edges of a box using several different
velocity and acceleration values.
The sizes of the arrays containing the values
are computed automatically by the macro
{\tt ARRAY_SIZE()}. On each pass through the inner loop, new
acceleration and velocity parameters are set using
{\tt setCartAccel()} and {\tt setCartVel()}. 
The rotational parameters are left unchanged by 
specifying {\tt F_UNDEF} or {\tt NULL} in the appropriate slot
of each primitive. 
To illustrate
the usage of the ``get'' routines,
the settings are
read back using {\tt getCartAccel()} and {\tt getCartVel()} and
printed.

The move commands to trace around the outside of
the box are generated by the subroutine {\tt moveSet()}
(\cref{14}); the program waits (at \cref{15}) for the drawing of each box
to finish before doing it again with another set of 
acceleration and velocity values. 

{\tt moveSet()} works by simply
queuing up motion requests to a list of positions given as an input
array. 
An alternative way of doing this
would be to use a single position equation and an input array of
target transform values. 
For instance, we could define a routine like this:
\begin{verbatim}
        moveTarget (mnp, tool, points, size)
        MANIP *mnp;
        TRSF_PTR tool, points[];
        int size;
        {
                POS *p;
                TRSF *target, *tl;
                int k;

                target = allocTrans (NULL, UNDEF);
                tl = allocTrans (NULL, UNDEF);
                *tl = *tool;
                p = makePosition (mnp, T6, tl, EQ, target, TL, tl);
                for (k=0; k<size; k++)
                 { *target = *points[k];
                   move (mnp, p);
                 }
                freePosition (p);
                freeTrans (tl);
                freeTrans (target);
        }
\end{verbatim}
All that this routine requires the user do is specify a tool transform
and an array of target points. The input transforms do not need
to be allocated with the {\tt allocTrans()} primitives
since they are not actually part of the position equation which is
used.

The program example ends by changing back to joint interpolated
motions (using {\tt setMod('j')}) and moving the robot
back to the start
position (\cref{16}). 
We do not bother to reset the acceleration or velocity
parameters, since {\tt setCartVel()} and {\tt setCartAccel()}
affect only Cartesian interpolated
motions (the equivalent primitives for joint interpolated motions
are {\tt setJointVel()} and {\tt setJointAccel()}). 
If one wants to simply scale
the robot's speed regardless of the interpolation
mode, then {\tt setSpeed()} can be used.

Note that before the program exits, it waits for the last motion
to finish using {\tt waitForCompleted(mnp)} (\cref{17});
otherwise, the program would probably
exit, and the robot would be stopped (abruptly!),\ before it actually reached
its last position. The final call to {\tt rcclRelease()} turns off the
trajectory generator, the argument {\tt YES} indicating that the
robot arm power should also be shut of.

\section{More Motion Parameters}

\subsection{Explicitly Setting Motion Times}

The program can override the
automatic computation of a motion's transition and duration times
($\tau$ and $\sigma$) with the primitive
\begin{verbatim}
        setTime (mnp, accelTime, travelTime)
          MANIP *mnp;
          float accelTime, travelTime;
\end{verbatim}
{\tt accelTime} 
and {\tt travelTime} are both in milliseconds.
{\tt accelTime} is actually one half the total acceleration
time; i.e., it is equal to $\tau$ multiplied by the control 
interval time (see figure \ref{trans}).
{\tt travelTime} is equal to $\sigma$ multiplied by the
control interval time. If either argument is specified as
{\tt F_DEFAULT}, then the corresponding time parameter is computed by
the trajectory generator, as usual.
{\tt setTime()} affects only the next motion requested.

This function
is very useful for motions involving time varying targets
(which will be discussed later).
Depending on how the target is time varying,
the system may not be able to compute the correct segment
time since this computation is based on the distance to the target position
at the {\it beginning} of the motion. In such cases the motion time
can be specified explicitly.
For example, suppose we have a target position {\tt spiral} which
contains a functionally defined transform causing it to track
a spiral path.
Then the code fragment
\begin{verbatim}
        setMod (mnp, 'c');
        setTime (mnp, 150.0, 2000.0);
        move (mnp, spiral);
\end{verbatim}
would cause the manipulator
to perform a spiraling motion for 2 seconds, beginning with
a 0.15 second transition time.

If we want to 
specify the transition time explicitly, then a call like
\begin{verbatim}
        setTime (mnp, 200.0, F_DEFAULT);
\end{verbatim}
will do the trick; this forces the transition time to be 0.2
seconds but lets the trajectory generator compute the segment duration
as normal, using the currently defined velocities.
It is sometimes necessary to specify a transition time of zero.
This can be useful for motions terminated on condition where the
reaction time is of primary importance.

A common thing to do is set the overall motion time and let the system
determine the transition time, as in
\begin{verbatim}
        setTime (mnp, F_DEFAULT, 1000.0);
\end{verbatim}
which specifies a total motion duration of 1 second.

Finally, we can specify a motion of indefinite duration by setting
the {\tt travelTime} to {\tt F_UNDEF}:
\begin{verbatim}
        setTime (mnp, F_DEFAULT, F_UNDEF);
\end{verbatim}
This is useful in cases where the motion of the arm is created by variable 
transforms within the target position (section \ref{bindings}), and we don't
care about reaching the target per se.

\subsection{Offsetting the Motion Target}

It is possible to specify an offset to a motion's target 
position. This
is useful if several poses are being planned in the 
vicinity of a particular target but we do not wish to create a separate
position equation for each pose.
The primitives
\begin{verbatim}
        distance (mnp, fmt, v[, v] ...)
          MANIP *mnp;
          char *fmt;
          float v;

        setDistance (mnp, offset)
          MANIP *mnp;
          TRSF *offset;
\end{verbatim}
both apply an offset to the target position
of the next motion request (and the next motion request only).
{\tt distance()} allows the offset to be specified as a
set of translation values and roll-pitch-yaw angles.
It takes a format string
listing the offset's components,
followed by a variable number
of arguments giving the associated values.
In the format string, each component is described with two letters.
The first letter can be either {\tt d} or {\tt r} (for {\it distance}
or {\it rotation}) and
the second letter can be either {\tt x}, {\tt y}, or {\tt z} (for each of
the principal axes).
For instance,
the specification string {\tt "dx rz"} indicates
a two-component offset consisting
of a translation along $x$ and a rotation around $z$. This string
would be followed by two arguments giving the corresponding values.
When computing the total offset, translation values are incorporated first,
followed by the rotations about the $z$, $y$, and $x$ axes (the same
order used for roll-pitch-yaw angles).

As an example, the code fragment
\begin{verbatim}
        distance (mnp, "dz", -30.);
        move (mnp, p);
        move (mnp, p);
        distance (mnp, "dz", -30.);
        move (mnp, p);
\end{verbatim}
implements a simple ``approach, reach, depart'' sort of
sequence along the $z$ direction of the tool.

The {\tt distance()} function can be used in conjunction with the
{\tt last} position equation in the {\tt MANIP} structure to execute
successive {\it relative} motions:
\begin{verbatim}
        move (mnp, p);
        distance (mnp, "dx", 100.0);
        move (mnp, mnp->last);
        distance (mnp, "dy", 100.0);
        move (mnp, mnp->last);
        distance (mnp, "dx", -100.0);
        move (mnp, mnp->last);
        distance (mnp, "dy", -100.0);
        move (mnp, mnp->last);
\end{verbatim}
This code fragment first moves the robot to the target specified by
{\tt p}, and then moves about a square in the $xy$ plane.

The components given to {\tt distance()} are turned into a transformation
which is applied to the target position in the TOOL coordinate frame.
For instance, if a target position is described by
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf C} \;\; {\bf P}
\]
with {\tt E} defining the TOOL frame, then a call to
{\tt distance()} would implicitly insert an offset transform {\tt SHIFT}
as follows:
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf C} \;\; {\bf P} \;\; {\bf SHIFT}
\]

{\tt setDistance()} behaves the same as {\tt distance()}, but
allows the program to
specify the offset transform directly; this provides a more
``program oriented'' interface.

Both {\tt setDistance()} and {\tt distance()} are canceled by
a {\tt movej()} request.

\subsection{Changing the Robot Configuration}

Most robot arms are capable of reaching a designated Cartesian position
in several ways. A particular instance of these redundancies is known as the
robot {\it configuration}. In particular, for PUMA robots, there are
three separate redundancies (right-handed/left-handed, elbow down/up,
and wrist flip/noflip), making a total of eight possible ways to
achieve any particular position.

When a robot is moving about 
in Cartesian coordinates, it usually retains its configuration from
one pose to the next.
The primitive
\begin{verbatim}
        setConf (mnp, conf)
          MANIP *mnp;
          char *conf;
\end{verbatim}
sets up an arm configuration change during the subsequent
motion.
This motion
{\bf has}
to be performed in
{\it joint}
mode, since a configuration change always involves moving through
a degenerate arm
position unreachable in
{\it Cartesian}
mode.
%\index {Cartesian mode}{}
Once the configuration change is obtained, the motions can again
be performed in
{\it Cartesian}
mode. {\tt setConf()} takes a string argument indicating the 
configuration change.
Different configurations are indicated by unique 
characters within the string.
For the PUMA arm, the configurations can be: shoulder
right-handed/left-handed  
({\tt r}/{\tt l});
elbow down/up ({\tt d}/{\tt u}); wrist flip/noflip ({\tt f}/{\tt n}).
For example, if the arm is in a left-handed, up, and noflip 
configuration ({\tt lun}),
then to change the wrist configuration to flip, we can write
\begin{verbatim}
        /* the arm is currently "lun" */

        setMod (mnp, 'j');    /* go in joint mode if it wasn't */
        setConf (mnp, "luf"); /* specify flip */
        move (mnp, new);      /* go "luf" */
\end{verbatim}
The configuration string does not have to be complete; it is permissible
to specify only those aspects of the configuration which are to desired
to be changed. For instance, in the example above, 
the call {\tt setConf (mnp, "f")} could have been used instead.

{\tt setConf()} is ignored by stop requests and handled only by the next
move request.
Also, if {\tt setConf()} is followed by
a {\tt movej()} request, then the {\tt setConf()} request will
be ``over-ruled'' by whatever configuration
is implicit in the {\tt movej()}.

The current manipulator configuration can be read back with
\begin{verbatim}
        getActiveConf (mnp, conf)
          MANIP *mnp;
          char *conf;
\end{verbatim}
which reads the robot's present configuration into the string {\tt conf}.

Robot configurations can also be described by a bitmask; for instance,
the kinematic routines (section \ref{kinematic}) use this representation.
The configuration codes for the PUMA are defined in the file
{\tt <puma_kynvar.h>}:
\begin{verbatim}
        PUMA_RIGHT_CONF -- right handed ('r')
        PUMA_DOWN_CONF  -- elbow down ('d')
        PUMA_FLIP_CONF --  wrist flip ('f')
\end{verbatim}

{\tt setConf()} returns a bitmask indicating which configuration bits
were specified. {\tt getActiveConf()} returns a bitmask equivalent to the
configuration string. 

For a given robot, the routines
\begin{verbatim}
        configToStr (str, mask, kyn)
          char *str;
          int mask;
          KYN *kyn;

        strToConfig (maskp, str, kyn)
          int *maskp;
          char *str;
          KYN *kyn;
\end{verbatim}
convert between the string and bit representations for configuration. 
{\tt kyn} is a pointer to the robot's {\tt KYN} structure, which is described
in section \ref{rcirbt}. Details on these routines can be found in the
reference manual.

The configuration representation used by RCCL should probably be improved.
For instance, there should be ways of indicating joint redundancies and
singularities (configurations are undefined at singularities). This may
be done when RCCL is upgraded to handle redundant manipulators.

\section{Synchronization}
%\index {synchronization}{}

As has been mentioned, motions are requested asynchronously to their
execution; a motion request is simply placed in a queue for servicing
by the trajectory generator at the next possible opportunity.
This makes it necessary to provide ways to explicitly coordinate the
planning level with the trajectory generator.

The most straightforward thing we can do is simply wait for the
trajectory generator to finish handling all the motion requests
in its queue. This can be done using the macro
{\tt waitForCompleted()} that was discussed earlier.
The code construction
\begin{verbatim}
        move (mnp, p1);
        move (mnp, p2);
        move (mnp, p3);

        waitForCompleted (mnp);
\end{verbatim}
will quickly queue three motion requests and then wait for them all to
complete. {\tt waitForCompleted()} actually blocks until the transition
{\it out of} the last motion has finished, so
the manipulator is guaranteed to be stationary
when it returns (unless {\tt TRACKING_MODE} has been selected).

Alternatively, we might want to wait for an individual motion request
to finish. To do this, we need to have a handle on the motion request.
Such a handle is returned by the motion primitives: all calls to
{\tt move()}, {\tt movej()}, or {\tt stop()} return a {\it motion ID},
an integer, that can be used to query information about the motion. 
In particular, the following primitives are useful:
\begin{verbatim}
        motionStatus (mid)
          int mid;

        float motionScale (mid)
          int mid;

        motionStartCode (mid)
          int mid;

        motionStopCode (mid)
          int mid;
\end{verbatim}
{\tt motionStatus()} returns either 
{\tt M_PENDING}, {\tt M_RUNNING}, {\tt M_PAUSED}, or {\tt M_FINISHED}
depending on whether the indicated motion is queued, is executing, 
has been paused (see section {pause}), or has completed.

{\tt motionScale()} returns a scalar value indicating
how far along the motion has progressed in its execution.
The scale value is 0.0 at the midpoint of the transition {\bf into} the
motion and 1.0 at the midpoint of the transition {\bf out of} the motion,
unless the transition was canceled prematurely, in which case the
final scale value will be less than 1.0. Because the system starts
computing the scale value at the beginning of the transition into the motion,
it will be slightly negative during the period between the beginning and
midpoint of the initial transition.

RCCL associates a code value with the start and completion
of a motion. The default code value
is {\tt ON_NORMAL}, which is defined in {\tt <rccl.h>}. 
Other code values can be specified
with the functions {\tt startMotion()} and {\tt stopMotion()}
(see below).

The primitives
\begin{verbatim}
        waitForStart (mid)
          int mid;

        waitForStop (mid) 
          int mid;
\end{verbatim}
respectively poll the indicated motion's status and block until
the motion has started executing or has finished executing.
For instance, the following code patterns are equivalent:
\begin{verbatim}
        id = move (mnp, p0);
        waitForStop (id);
        id = move (mnp, p1);
        waitForStop (id);
\end{verbatim}
or
\begin{verbatim}
        move (mnp, p0);
        waitForCompleted (mnp);
        move (mnp, p1);
        waitForCompleted (mnp);
\end{verbatim}

To further demonstrate the use of these primitives, consider the following
(somewhat contrived) example:
\begin{verbatim}
        MANIP *mnp;
        FILE *fp;
        POS_PTR posA, posB, posC;
        int id0, id1;

        id0 = move (mnp, posA);
        id1 = move (mnp, posB);
        move (mnp, posC);

        waitForStop (id0);

        while (motionStatus (id1) == M_RUNNING)
         { fprintf (fp, "scale = %f\n", motionScale (id1));
           fprintTrsf (fp, "%m\n", mnp->t6);
         }
\end{verbatim}
This program queues three motion requests, waits for the first one
to complete, and then begins to record the successive {\bf T6} values
(along with the motion scale value) for the second motion in the file
{\tt fp}.

As another example, consider the ``approach, reach, depart'' sequence
described earlier. If we want to coordinate the planning level of the program 
so that it does something (such as open a gripper at the approach 
point and close it at the reach point), then we can use a code segment that
looks something like this:
\begin{verbatim}
        int approachId, reachId;

        distance (mnp, "dz", -30.);
        approachId = move (mnp, p);
        reachId = move (mnp, p);
        distance (mnp, "dz", -30.);
        move(p);

        waitForStop (approachId);
        OPEN_HAND (mnp);
        waitForStop (reachId);
        CLOSE_HAND (mnp);
\end{verbatim}
The motion IDs
for the moves to the approach and reach positions are recorded
in {\tt approachID} and {\tt reachId}.
The program waits until the manipulator arrives at the
{\it approach} position,
opens the gripper, waits again for the manipulator
to arrive at the reach position,
and closes the gripper.

The above example introduces the macros 
\begin{verbatim}
        OPEN_HAND(mnp)
          MANIP *mnp;

        CLOSE_HAND(mnp)
          MANIP *mnp;
\end{verbatim}
which open and close the gripper for the specified manipulator
(this hand control interface is quite rough and could use some
improvement).

To wait for an arbitrary condition or delay for a specific period of time,
the following primitives are available:
\begin{verbatim}
        waitFor(exp)

        waitWhile(exp)

        delay(msec)
          float msec;
\end{verbatim}
The first two are macros:
{\tt waitFor(exp)} waits for the indicated expression to
return true, and {\tt waitWhile(exp)} waits for the indicated expression to
return false. {\tt delay()} puts the program to sleep for
the indicated number of milliseconds; it can be called only from the
planning level.

\subsection{Canceling and Controlling Motions}\label{canceling}

The functions
\begin{verbatim}
        stopMotion (mid, code) 
          int mid, code;

        startMotion (mid, code) 
          int mid, code;

        setMotionHold (mnp)
          MANIP *mnp;
\end{verbatim}
may be used to control individual motion requests.

{\tt stopMotion()} cancels the indicated motion and sets its
motion stop value to {\tt code}.

{\tt startMotion()} gives the indicated
motion permission to begin and gives it a start value equal to
{\tt code}. This may seem a bit obscure, since normally
all motions are queued with permission to begin and are given
a start value of {\tt ON_NORMAL}. 
However, calling {\tt setMotionHold(mnp)} before a motion request
is issued will remove its permission to begin,
so that
when it reaches the head of motion queue, and thus would otherwise be
ready to run,
it will not do so until permission is granted by a call
to {\tt startMotion()}. This is mainly useful for starting motions
from the control level (which is not permitted to issue actual
motion requests).

We now consider the motion ID values. While
a motion ID which is automatically
allocated is always unique,
only its low order bits are used to point to the
internal cell containing the motion's status. Because memory space is limited,
the 
number of these cells is also limited, and the status cell associated
with a motion ID will be re-used every {\tt NUM_SYSMFLAGS} motions.
This number is large enough to accommodate most applications, but in
case the programmer desires to keep motion status information around
indefinitely, the function
\begin{verbatim}
        setMotionFlag (mnp, mid) 
          MANIP *mnp;
          int mid;
\end{verbatim}
can be used to explicitly set the motion ID of the next motion to be
{\tt mid}. This value must be between 0 and 
$\hbox{\tt NUM_USRMFLAGS} - 1$. The use of these motion IDs
is strictly
under application control; RCCL will never automatically assign 
a motion ID in this range.

To illustrate some of this, consider
another example involving two robots:
\begin{verbatim}
        #define FLAG1   1
        #define FLAG2   2

        MANIP_PTR mnp1, mnp2;
        POS_PTR posA, posB;

        setMotionFlag (FLAG1);
        move (mnp1, posA);

        setMotionFlag (FLAG2);
        move (mnp2, posB);

        while (motionScale (FLAG1) < 0.5)
           ;

        stopMotion (FLAG2, 0);
\end{verbatim}
The idea here is that we have two robots, respectively
controlled via {\tt MANIP} structures pointed to by
{\tt mnp1} and {\tt mnp2}. We start the first
robot moving toward {\tt posA} with {\tt FLAG1} explicitly set as
the motion ID
(note that we do not store the ID returned
by the motion request since we know what it is).
The second robot is then started moving towards {\tt posB} with the
motion ID {\tt FLAG2}. We then monitor the scale value of the first
motion, and when it is more than half completed, cancel the
motion on the second robot with a code value of 0.

\subsection{Controlling the Current Motion and Motion Queue}

The primitive
\begin{verbatim}
        stopCurrentMotion (mnp, code)
          MANIP *mnp;
          int code;
\end{verbatim}
may be used
to cancel whatever motion happens to be executing currently, regardless
of its motion ID.

A few functions are available for controlling the motion queue:
\begin{verbatim}
        flushMotionQueue (mnp)
          MANIP *mnp;

        numMotionsQueued (mnp)
          MANIP *mnp;

        checkMotionQueue (mnp, pos)
          MANIP *mnp;
          POS *pos;

        setMotionQueueSize (mnp, size)
          MANIP *mnp;
          int size;

        getMotionQueueSize (mnp)
          MANIP *mnp;
\end{verbatim}
{\tt flushMotionQueue()} cancels all pending motions on the motion
queue. It will not, however, abort the current motion, so the
proper way to ``kill everything'' is with a code fragment like this:
\begin{verbatim}
        flushMotionQueue (mnp);
        stopCurrentMotion (mnp, STOPCODE);
\end{verbatim}
The motion queue is flushed first so that when the current
motion is aborted, the system will not take another off
the queue and start executing it.

{\tt numMotionsQueued()} returns the
number of motions pending on the motion queue, and 
{\tt checkMotionQueue()} returns true if 
there is enough space on the queue to handle a motion request
to the target position {\tt pos}.

The maximum size of the motion queue can be set with {\tt
setMotionQueueSize()} 
and read back with {\tt getMotionQueueSize()}. Setting the 
maximum queue size can be a useful synchronization tool in that different
queue sizes will produce different blocking behaviors for the
motion requests. Setting the motion queue size to 0 will cause every
motion request to block until the associated motion has been completed
(equivalent to doing a {\tt waitForCompleted()} after every motion
request).
Setting the motion queue size to 1 will cause every motion request to
block until its motion is being executed. A
general motion queue size of $n$ will cause the move primitives
to block until there are only $n - 1$ motions still pending on the
queue. Consider the following example:
\begin{verbatim}
        MANIP *mnp;
        POS_PTR posA, posB, posC, posD, posE;

        setMotionQueueSize (mnp, 0);
        move (mnp, posA);

        /* nothing on queue; move to posA is now complete */

        setMotionQueueSize (mnp, 1);
        move (mnp, posB);

        /* nothing on queue; move to posB has at least started */

        setMotionQueueSize (mnp, 3);
        move (mnp, posC);
        move (mnp, posD);
        move (mnp, posE);

        /* no more than 2 motions on queue; */
        /* move to posC has at least started */
\end{verbatim}
We first set the motion queue size to 0, causing {\tt move(mnp, posA)}
to block until completion. The queue size is then set to 1, which means
that the call {\tt move (mnp, posB)} will block until the motion is
at least being executed. Setting the queue size to 3 means that subsequent
move requests will block until there are no more than 2 motions on
the queue; this means that {\tt move (mnp, posE)} must block until
the move to position {\tt posC} is at least being executed.

The motion queue can be set as large as {\tt MAX_MQ_ENTRIES}. 
The default queue size can be obtained by calling {\tt getMotionQueueSize()}
immediately after the {\tt MANIP} structure has been created.

As another example of how the motion request queue interacts
with the planning level, consider the following program:
\begin{verbatim}
        TRSF_PTR z, e , b;
        POS_PTR p;
        float iz;

        z = allocTransXyz ("Z",  0., 0., 864.);
        e = allocTransXyz ("E" , 0., 0., 170.);
        b = allocTransRot ("B", 600., 128., 800., yunit, 180.);

        p = makePosition ("P", z, t6, e, EQ, b, TL, e);

        while (1)
         { printf ("Enter Z increment> ");
           scanf ("%f", &iz);
           b->p.z += iz;
           move (mnp, p);
         }
\end{verbatim}
This allows the user to continuously adjust the $z$ position
of the robot. Every time an increment for $z$ is entered, a new motion
request to handle that increment will be queued, using the current
value of the {\tt b} transform.

If the user enters data faster than the manipulator
can move to the goal positions,
several motion requests will be queued up.
If the user stops entering data, all the requests will eventually be 
executed,
the manipulator will be brought to rest, and the program
will block at the {\tt scanf()} call.
If the data is provided by continuous stream {\tt fp}, then the same program
could look something like:
\begin{verbatim}
        while (fread (b, sizeof(TRSF), 1, fp) != NULL);
         { move (mnp, p);
         }
\end{verbatim}

\subsection{Getting UNIX signals on motion completion}

A useful feature of RCCL is the ability to request a UNIX signal
to be delivered whenever (1) all requested motions complete
or (2) a particular motion completes (these are the same conditions
which cause {\tt waitForCompleted()} or {\tt waitForStop()} to return
true). The routines to set up the signals are
\begin{verbatim}
        setCompletedSig (mnp, signum)
          MANIP *mnp;
          int signum;

        setStopSig (mnp, signum, mid)
          MANIP *mnp;
          int signum, mid;
\end{verbatim}
{\tt setCompletedSig()} requests that the signal {\tt signum} be sent to
the planning task the next time the trajectory
generator finishes executing all the motions on the queue.
{\tt setStopSig()} requests that the signal {\tt signum} be sent to
the planning level task whenever the motion specified by the
ID {\tt mid} finishes.

Consider the following example:
\begin{verbatim}
        #include <rccl.h>
        #include <signal.h>

        handler()
        { 
          printf ("All motions completed\n");
        }

        robottask()
        {
          ... initializations and other things ...

          signal (SIGUSR1, handler);

          setCompletedSig (mnp, SIGUSR1);

          move (mnp, p0);
          move (mnp, p1);
          move (mnp, p2);

          printf ("Hit <CR> to quit:\n");
          getchar(); 
        }
\end{verbatim}
This program will quickly queue three motion requests, and then
block waiting for a character. When all three motions have 
completed, the message {\tt All motions completed} will be 
asynchronously printed to the screen.

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

This program has the manipulator trace out a small
hexagonal grid, or ``honeycomb'' pattern (see figure \ref{hexgrid}).
The robot does
one column in the grid at a time, and before doing each one goes
to a ``pickup'' position, as though it were reloading a tool
(see figure \ref{hexreload}).
When each column is completed, 
a signal is sent to the planning level that causes a message to be
printed.

This program is a bit complex for example purposes, but it
does illustrate a wide variety of capabilities.

% try to get a better page break
\vskip 0pt plus .5in\penalty -1000\vskip 0pt plus -.5in\relax

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

}
\medskip

\begin{figure}[tbh]
%\rect 4.25truein by 5truein%
\boxedsizedfig{4.25truein}{5truein}%
{\psfig{figure=figs/hexgrid.idr}}%
{Robot drawing out a hexagonal ``honeycomb'' pattern.}%
\label{hexgrid}%
\end{figure}


\begin{figure}[tbh]
%\rect 4.25truein by 5truein%
\boxedsizedfig{4.25truein}{5truein}%
{\psfig{figure=figs/hexreload.idr}}%
{Position the robot moves to
in between drawing each column of the honeycomb.}%
\label{hexreload}%
\end{figure}


The file {\tt <rccl.h>} is included as usual. {\tt <signal.h>} and 
{\tt <math.h>} are UNIX files included to get the definition
of the signal {\tt SIGUSR1} and define the types for routines
{\tt sin()} and {\tt cos()} (\cref{1}). The signal handler which will be
used to print the column completion message is defined at (\cref{2}).

The program declares a few variables and pointers, gets the name of the
robot to be controlled and its starting position (\cref{3}), then
allocates several transforms and position equations (\cref{4} and \cref{5}).
Except for the end-effector transform {\tt e}, all of the transforms
will be initialized later in the program.
The position {\tt start} describes
the position reached after moving to the joint angles
{\tt rcclpark}, and is defined simply by
\[
{\bf T6} \;=\; {\bf PARK}
\]
{\tt reload} is the place the robot moves to before tracing
out each column, and is defined by the equation
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf PICKUP}
\]
where {\bf E} describes the manipulator's end-effector transform.
The most complicated position is {\tt center}, defined by
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf PARK} \;\; {\bf TOP} \;\; {\bf DOWN}
\]
which specifies the center point of each hexagon the robot traces out.
{\bf TOP} locates the top of each column
with respect to the initial frame defined by {\bf PARK}. The
transform {\bf DOWN}
describes, with respect to the frame TOP,
a translation in the $xy$ plane
from the top of the
column down to the center of a particular hexagon.
As the program traces out the different hexagons, it locates the 
center of each one by changing the values for {\tt top} and {\tt down}
appropriately.

The program begins in the usual fashion by setting up the
manipulator, starting the trajectory generator,
and moving the robot to the initial angles described
by {\tt rcclpark} (\cref{6} and \cref{7}).
At \cref{8} the program initializes {\tt park} to the
current value of {\bf T6} by reading the {\tt here} field
of the {\tt MANIP} structure.
{\tt pickup} is then defined to be a position
``off to the side'' of {\tt park}; specifically, {\tt park} rotated
by $90^\circ$ in world coordinates 
about the $z$ axis of the robot's base, followed by
a $30^\circ$ rotation about the local $x$ axis (\cref{9})
(see figure \ref{hexreload}).
{\tt top} is initialized to describe the top left-hand corner of the hex grid.
Specifically, this involves a translation in the $xz$ plane followed
by a rotation of $90^\circ$ about the
$x$ axis so that the robot's tool tip points horizontally at the
grid (\cref{10}) (see figure \ref{hexgrid}).
Because of this last rotation, the $xy$ plane of the frame TOP is
parallel to the $xz$ plane of the initial frame PARK.
At \cref{11} the program sets the speed
for joint interpolated motions to be double that of Cartesian interpolated
motions, sets the Cartesian velocities explicitly, and sets up the
signal handler for the end-of-column signal. 
The variable {\tt sin_60} initialized at
\cref{12} is introduced to make the program more readable.

A loop begins at \cref{13} that causes the robot to trace out each of
the {\tt numCols} columns in the grid.
The robot first moves to the {\tt reload} position, where
it does an approach-move-stop-depart sequence \cref{15}. The 
program synchronizes with the arrival of the robot
at reload by waiting on the motion ID {\tt rid} (\cref{16})
after which it opens the robot's hand, waits half
a second, and then closes the hand.

\begin{figure}[tbhp]
%\rect 6truein by 7truein
\boxedsizedfig{6truein}{7truein}
{\psfig{figure=figs/tracehex.eps}}%
{Path followed by the robot as it traces out one column
in the hex grid, projected onto the $xy$ plane of the frame TOP.
The rounding due to transitions is ignored.}%
\label{tracehex}
\end{figure}

The robot then moves to the top of the next column in the grid.
Since it is better able to
reach the grid with its wrist in the {\it flip} configuration, this
configuration is explicitly requested (and later unrequested)
with calls to {\tt setConf()} and {\tt getConf()} (\cref{14} and
\cref{17}). 
Each column is set adjacent to the previous one
by shifting the $x$ coordinate
of {\tt top} by $2 \sin(60^\circ) \hbox{\tt hexsize}$, where {\tt hexsize}
is the length of each hexagon's side.
{\tt down}, which is used to specify the
center of each hexagon relative to the top of the column, is 
initially cleared
(\cref{18}).
The subloop at \cref{19} uses the routine {\tt traceHex()}
(discussed below) to create the hexagons. 
For each loop iteration,
the hexagon's center is located by increasing the $y$ coordinate 
of {\tt down} by $3/2 \hbox{\tt hexsize}$ and shifting the
$x$ coordinate either to the right or left 
by $\sin(60^\circ) \hbox{\tt hexsize}$ (\cref{19}).
The tracing of a single column is illustrated by figure \ref{tracehex}.

The routine {\tt traceHex()} generates the motion requests necessary
to trace out a hexagon in the $xy$ plane of a position {\tt p}.
All the motions are specified relative to the center {\tt p} using
an offset transform ({\tt offset}) and the routine {\tt setDistance()}.
It begins by issuing a move to an
approach position located {\tt approach} millimeters
away from the center point along the $z$ axis (\cref{23}).
{\tt setMod()} is then used to ensure that the remaining motions
are done in Cartesian mode (\cref{25}). To 
improve the modularity of {\tt traceHex()}, no assumptions are made about the
previous interpolation mode. This means that the mode should
be ``saved and restored'', which is done with calls to
{\tt getMod()} and {\tt setMod()} (\cref{24}, \cref{30}). 
After the approach move is requested,
the $z$ offset is removed (\cref{26}) and a set of moves are generated
to take the robot around the corners of the hexagon.
Each corner
point is determined by computing an appropriate $X,Y$ offset from the
hexagon center as a function of an angle {\tt theta} (\cref{27}).
The actual trajectory will be slightly ``rounded out'' because of
path transitions, and the corner points will be correspondingly
undercut,
unless the robot is slowed down enough to permit a zero transition time
to be specified.
Lastly, the robot is moved back to the approach position (\cref{29}).
The ID returned by this last motion is saved and returned to the caller.
If the routine argument {\tt flag} has been set to a value
other than {\tt UNDEF}, then it is used as the motion ID for the
last motion. This is useful in case the caller needs to know the motion ID
{\tt before} {\tt traceHex()} is called.

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

This example shows how to create a simple program to move the
robot around in joint coordinates and change its configuration. 
For simplicity, it does not attempt to deal with prismatic joints.
The
program is interactive and illustrates the use of the ``C-tree
matcher'', an automatic command completion keyboard interface used
by many programs in the RCCL/RCI system. 

\cverbatimfile{exampleCode/jmove.c}
{\it 
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA robots,
and lives at \hfil\break {\filefont \$RCCL/demo.rccl/jmove.c}.

}
\medskip

The most complicated thing to explain about this program is the
interface code for the ``C-tree matcher''. We feel this is
justified because this interface is ubiquitous throughout the RCCL/RCI
system. To avoid learning about the C-tree matcher,
just skip over the next few paragraphs.

The C-tree matcher is a command completion input routine. When
called, it presents the program operator with a prompt and allows
him/her to type in only a selected set of keywords or values.
Since the matcher knows what inputs are valid, it can 
assist the user by providing a menu of what is legal to type (if the user
hits `{\tt ?}') or by expanding input to the next branch point
(if the user hits {\tt <space>}). The matcher returns when a valid
sequence has been entered and the user hits {\tt <return>}. It returns
to the program the string of keywords which the user typed, plus
any input values it collected. While this sounds cumbersome, in
practice it is not. Full documentation on the C-tree matcher,
for both the end user and the programmer, is available 
in {\filefont \$RCCL/doc/CtreeMatch.doc}.

The program begins as usual by including {\tt <rccl.h>} and declaring
miscellaneous variables and pointers.
Definitions relevant to the C-tree matcher are contained in the
include file {\tt <ctree.h>} (\cref{1}).
The name of the robot to be controlled is obtained, in this program, from the
command line rather than from the system routine 
{\tt getDefaultRobot()} (\cref{2}).

The first main thing the program does is describe
to the C-tree matcher the set of permitted inputs. This is done using
a tree of keywords and input values, specified by a long string
argument given to the routine 
{\tt tree_match_parse()} (\cref{3}). The inputs allowed by
this program are
\begin{verbatim}
        quit
        show
        speed <speedValue>
        move all
        move <jointNumber> by <angleValue>
        move <jointNumber> to <angleValue>
        setConf <configString>
\end{verbatim}
Successive values or keywords are 
indicated within the specification string by parentheses:
\begin{verbatim}
        <keyword or value> '(' <following keywords or values> ')'
\end{verbatim}
Whenever the input requires actual values, the C-tree matcher will
parse the value according to a prescribed format
and place it in a variable whose address has
been given to {\tt tree_match_parse()}. Allowed formats are
{\tt \%d} (integer), {\tt \%f} (float), {\tt \%s} (string), and
{\tt \%x} (hex integer). The address through which the value should be stored
is placed in the argument list to {\tt tree_match_parse()} by
breaking the specification string, inserting the address, and restarting
the specification string:
\begin{verbatim}
        string spec ... %d", &integerVal, " ... more string spec
\end{verbatim}
Integer and float specifications are followed by (optional) lower and upper
bounds, and all value specifications may also be followed by a help
prompt (contained within {\tt \\" \\"}) that appears when the user
hits `{\tt ?}'.

{\tt tree_match_parse()} digests the input specification and
returns a handle ({\tt keytree}) that is later used by
{\tt tree_match()}, which does the actual reading of input.  If an
error was found in the input specification, {\tt tree_match_parse()}
returns 0, and a string showing the neighborhood where
the error occurred can be obtained with the routine {\tt tree_match_err_at()}.
(\cref{4}).

Now back to RCCL. After the program has initialized the C-tree matcher,
it calls {\tt rcclCreate()} and {\tt rcclStart()} to set up
the trajectory generator and start it (\cref{5}).
It then initializes the variable {\tt angles}
to the current values
of manipulator joint angles, which are stored in the {\tt j6} field
of the {\tt MANIP} structure (\cref{6}). A loop is then entered 
which calls {\tt tree_match()} (\cref{7}) to
solicit commands from the user's terminal, until the final
{\tt quit} command is received (\cref{22}).

Commands returned by {\tt tree_match()} consist of sequences of
keywords and are placed in the buffer {\tt key_buf}. The macro
{\tt COMMAND(str)} does a {\tt strcmp()} between {\tt str} and
the contents of {\tt key_buf} to determine what command was entered.
The commands that may be returned to this program are
\begin{verbatim}
        quit
        show
        speed
        move all
        move by
        move to
        setConf
\end{verbatim}

The {\tt show} command (\cref{8}) causes the program
to print out the robot's current speed setting,
configuration, {\bf T6} transform value, and joint angles.
Note that the joint
angles are converted to degrees before being printed out; all joints
are assumed to be revolute as mentioned earlier.

The {\tt speed} command (\cref{9})
sets the robot's speed parameter,
scaling the speed at which the robot responds to further motion commands.
The variable {\tt speed} is set during command input by {\tt tree_match()}.

The {\tt move all} (\cref{10}) command is used to move the robot to a 
particular set of joint values. It prompts the user
for a set of joint angles, reads these in from the keyboard,
converts them to radians, and requests a call to {\tt movej()}
by setting the variable {\tt movejRequest} (\cref{11}).

The {\tt move by} command (\cref{12})
moves one joint by a specified number of
degrees. The joint number and the amount to move it by are read 
into the variables {\tt jnt} and {\tt angval} by {\tt tree_match()}
when the command is entered.
The appropriate field in the joint target variable {\tt angles} 
is changed, and a call to {\tt movej()} is
requested by setting {\tt movejRequest}.

The {\tt move to} command (\cref{13})
is similar to the {\tt move by} command
except that it moves a joint {\bf to} a specified value.

All of the {\tt move} commands change the joint target variable
{\tt angles} and bump the variable {\tt movejRequest}, which is
examined at the bottom of the loop (\cref{19}). If set, the program uses 
{\tt movej()} to queue a motion request to 
{\tt angles}. The value returned by {\tt movej()} is
stored in {\tt newMid}. If this value is negative, then {\tt movej()} 
is known to have failed
and the appropriate error message(s) are printed and the error stack is
cleared with a call to {\tt printErrors()} and {\tt clearErrors()} 
(\cref{20}).
(see section \ref{errorStack} for a discussion of the error stack).
Otherwise, the request succeeded and {\tt newMid} contains its
synchronization flag. The program next checks to see if there
has been any previous call to {\tt movej()} ({\tt mid != UNDEF}),
and if so, if the resulting motion has completed yet. If it has not, the
previous motion is 
aborted with a call to {\tt stopMotion()} using the old motion's ID 
value (\cref{21}). Doing
this {\bf after} the call the {\tt movej()} allows for a smooth transition
from any previous motion to the new motion. It also provides a
nice illustration of the use of the synchronization primitives.

The last command in {\tt jmove} is {\tt setConf} (\cref{14}), which
is used to change the robot's current
kinematic configuration. It first uses {\tt stopCurrentMotion()}
and {\tt waitForCompleted()} to stop any current manipulator
motion (\cref{15}).
A string describing the new desired configuration is read into
{\tt conf} by {\tt tree_match()} when the command is entered,
and is used directly as an argument to {\tt setConf()} (\cref{16}).
A motion is then requested to the robot's current 
position (\cref{17}), using the
the position equation {\tt mnp->last} built in to the {\tt MANIP} structure.
Internally, this will cause 
the trajectory generator to compute the target
joint angles for the motion using the new configuration and generate
a joint interpolated motion to these new angles. The program waits for
the motion to finish with another call to {\tt waitForCompleted()} and
then updates the value of {\tt angles} (\cref{18}).

\newpage
\done
