\input rccluginit.latex

\chapter{Force Control and Motion Limit Detection}

In assembly tasks, objects are required to be brought into contact
and motions have to be stopped when the collision occurs.
Once objects are in contact the task is said to be {\it constrained\/}
because arbitrary motions are no longer possible in every direction.
When the task is constrained, the arm must exert
forces on objects and can no longer be purely position servoed for
all six degrees of freedom.
Instead, the degrees of freedom (DOFs) along or about which the constraints
occur must {\it comply\/} to the interaction forces.
When moving to and from contact situations, guarded motions are usually
performed. The limit conditions for these guarded motions are typically
force thresholds (when contact is anticipated), and possibly
trajectory error thresholds (when the robot is complying but there
is a possibility it might ``slip off'' the contact surface).

Multi-RCCL does not have an explicit built-in capability for
performing compliant motions or monitoring specific
motion limit conditions.
The original
version of RCCL did provide a simple force algorithm based on the
old Paul and Shimano method ([Paul and Shimano 1976]),
but this does not work well on
robots which lack joint level force sensors, and even when such
sensors are available, it is
approximate and unstable.
Old RCCL also provided built-in checking for force and displacement limits,
which allowed automatic cancellation of a motion
when either observed forces or displacements from the desired
trajectory exceeded specified threshold values.

Explicit support of this capability was dropped
because it required making too many assumptions
about what sensors were available and what
algorithms might be suitable for different applications.
This does not restrict the
overall functionality
of the system, however, since
both compliant motion and the limit checking can be easily implemented by
application code using monitor functions and functionally defined transforms.
In fact, the ease with which such functionality can be embedded within RCCL
has been a principal reason for its popularity.
The RCCL sites at GE/ATL and JPL have both
implemented their own force control strategies within RCCL,
based, in both cases, on position accommodation techniques such as those
described by
[Maples and Becker 1986].
Example force control programs written at JPL are included at the
end of this section.

As a convenience for the programmer (and for possible future extensions)
Multi-RCCL still provides {\it interface\/}
routines which allow force control and limit
specifications to be described for particular motions,
and read back when necessary by application code responsible for their
implementation. These routines will be discussed below and their
use illustrated by example programs.

\section{Limit Specification Routines}

Force or displacement limits for the next motion
can be specified by the
routines
\begin{verbatim}
        limit (mnp, format, value1, value2, ... )
          MANIP *mnp;
          char *format;
          float value1, value2, ...

        setForceLimit (mnp, mask, flimit)
          MANIP *mnp;
          unsigned long mask;
          FORCE *flimit;

        setDispLimit (mnp, mask, dlimit)
          MANIP *mnp;
          unsigned long mask;
          DIFF *dlimit;
\end{verbatim}

{\tt limit()} uses a format string, containing a set of two letter codes
separated by white space, to state what types of limits to check
for. Translational force limits (along the principal axes)
are indicated by the codes {\tt fx}, {\tt fy}, and {\tt fz}.
Torque limits (about the principal axes) are indicated by the codes
{\tt tx}, {\tt ty}, and {\tt tz}. Likewise,
displacement limits along or about the principal axes are indicated
by {\tt dx}, {\tt dy}, {\tt dz}, {\tt rx}, {\tt ry}, or {\tt rz}.
For each given limit, the routine takes an additional argument
describing the associated threshold value.

For instance, a force limit of 10.0 Newtons along the $x$ and
$y$ axes, and a displacement limit of $30^\circ$ about the $z$ axis,
could be specified as
\begin{verbatim}
        limit (mnp, "fx fy rz", 10.0, 10.0, 30.0);
\end{verbatim}

A more ``machine oriented'' way of describing the same thing is provided by
the routines {\tt setForceLimit()} and {\tt setDispLimit()}. {\tt
setForceLimit()} uses a bit mask to specify the degrees of freedom
along (or about) which force limits should be monitored; the
limit value for each selected DOF is obtained from the corresponding field in
the argument {\tt flimit}. The bit mask should be assembled from the
following codes:
\begin{verbatim}
        ALONG_X
        ALONG_Y
        ALONG_Z
        ABOUT_X
        ABOUT_Y
        ABOUT_Z
\end{verbatim}
Displacement limits are set in the
same way using {\tt setDispLimit()}. The sample usage of {\tt limit()}
given above could be coded using these routines as:
\begin{verbatim}
        FORCE flimit;
        DIFF dlimit;

        flimit.f.x = 10.0;
        flimit.f.y = 10.0;
        setForceLimit (mnp, (ALONG_X|ALONG_Y), &flimit);
        dlimit.m.z = 30.0;
        setDispLimit (mnp, ABOUT_Z, &dlimit);
\end{verbatim}

Limit specifications are made by the planning level and
are valid only for the duration of the next requested motion.

Since RCCL does not actually implement the limit detection, some
part of the application program (a monitor function most likely)
will have to read back the limit specifications.
This is done with the routines
\begin{verbatim}
        getActiveForceLimit (mnp, mask, flimit)
          MANIP *mnp;
          unsigned long *mask;
          FORCE *flimit;

        getActiveDispLimit (mnp, mask, dlimit)
          MANIP *mnp;
          unsigned long *mask;
          DIFF *dlimit;
\end{verbatim}
{\tt getActiveForceLimit()} returns (through {\tt mask}) a bit mask
describing the force limits
for the motion currently active on {\tt mnp}. The associated thresholds
are returned in the corresponding fields of {\tt flimit}.
Similarly, {\tt getActiveDispLimit()} returns the currently
active displacement limits.

A simple monitor function that checks for displacement limits could
be written as follows:
\begin{verbatim}
        dispMonitor (arg, mnp)
        int arg;
        MANIP *mnp;
        {
                unsigned long mask;
                DIFF limit;
                TRSF disp;
                float rz, ry, rx;

                getActiveDispLimit (mnp, &mask, &limit);

                if (mask)
                 { multRiTrsf (&disp, mnp->t6o, mnp->t6);
                   trsfToRpy (&rz, &ry, &rx, &tmp);

                   if (((mask & ALONG_X) && FABS(disp.p.x) > limit.t.x) ||
                       ((mask & ALONG_Y) && FABS(disp.p.y) > limit.t.y) ||
                       ((mask & ALONG_Z) && FABS(disp.p.z) > limit.t.z) ||
                       ((mask & ABOUT_X) && FABS(rx) > limit.r.x) ||
                       ((mask & ABOUT_Y) && FABS(ry) > limit.r.y) ||
                       ((mask & ABOUT_Z) && FABS(rz) > limit.r.z))
                    { stopCurrentMotion (mnp, ON_LIMIT);
                    }
                 }
        }
\end{verbatim}
This is a permanent monitor function that runs all the time.
It checks to find out what limit
specifications are currently in effect for its associated manipulator.
If there aren't any ({\tt mask == 0}), it saves
computation by simply returning. Otherwise,
it computes the difference between the desired trajectory and the real
trajectory by
multiplying the output
value of {\bf T6} (given by the {\tt t6} field of the {\tt MANIP}
structure) by the inverse of the observed value of
{\bf T6} (given by the {\tt t6o} field;
the mode {\tt T6O_EVAL} (section \ref{modes}) has presumably
been set so that the trajectory generator will in fact
maintain {\tt mnp->t6o}). The rotational difference is
converted into yaw, pitch, and roll angles so that it may be
easily compared with the displacement thresholds along each axis.
For each selected DOF, the trajectory error is then compared with the
threshold and, if it is exceeded, the motion is canceled with the
code {\tt ON_LIMIT}.

Because only the application software uses the values set by
the limit routines, they can be used to implement things different
from their original defined purpose. For instance, {\tt setDispLimit()}
could be used to implement a velocity limit rather than a trajectory
error limit. A monitor function which did this would look the same as
the one above, except that it would have to compute a velocity transform
instead of a displacement transform. It could do this by keeping the old
value of {\bf T6} around in a static variable, multiplying it by the new
value of {\bf T6} every cycle, and then scaling the outputs to
units-per-cycle rather than units-per-second:
\begin{verbatim}
        TRSF oldT6;
        float scale;

        ...

        scale = rcclGetInterval() / 1000.0;

        multRiTrsf (&disp, &oldT6, mnp->t6);
        trsfToRpy (&rz, &ry, &rx, &tmp);
        rx *= scale;
        ry *= scale;
        rz *= scale;
        disp.p.x *= scale;
        disp.p.y *= scale;
        disp.p.z *= scale;

        oldT6 = *mnp->t6;
\end{verbatim}

\section{Compliance Specification Routines}

Compliant motions can be specified with routines similar
to the limit routines:
\begin{verbatim}
        comply (mnp, format, value1, value2, ...)
          MANIP *mnp;
          char *format;
          float value1, value2, ...

        lock (mnp, format)
          MANIP *mnp;
          char *format;

        setComply (mnp, mask, bias)
          MANIP *mnp;
          unsigned long mask;
          FORCE *bias;
\end{verbatim}
{\tt comply()} takes a format string indicating which DOFs should be
put into compliant mode. The codes are the same as those used to describe
force limits with the {\tt limit()} routine: {\tt fx}, {\tt fy}, {\tt fz},
{\tt tx}, {\tt ty}, and {\tt tz}. For each selected DOF, an additional
argument is provided indicating the bias force to be
associated with the compliance.

A compliance specification for a particular DOF
takes effect with the next requested motion and stays in effect
for all subsequent motions until explicitly canceled. To take a
particular DOF out of comply mode, the primitive {\tt lock()} can
be called, where the DOFs to be locked are specified using the same
format as for the {\tt comply()} routine.
For example, suppose the program wishes to establish a compliance along
the $z$ axis with a bias force 10.0 (Newtons) for the next three moves,
and in addition, specify a zero-bias compliance about the $x$ and $y$
axes for the second motion. This could be specified as follows:
\begin{verbatim}
        comply (mnp, "fz", 10.0);
        move (mnp, p1);
        comply (mnp, "tx ty", 0.0, 0.0);
        move (mnp, p1);
        lock (mnp, "tx ty");
        move (mnp, p3);
        lock (mnp, "fz");
\end{verbatim}

Compliance can also be described in a more
``machine oriented'' way using the primitive {\tt setComply()}, which takes a
mask selecting the desired comply DOFs and a {\tt FORCE} type
argument containing the corresponding biases. Using this, the above example
could be coded as:
\begin{verbatim}
        FORCE bias;

        bias.f.z = 10.0;
        setComply (mnp, ALONG_Z, &bias);
        move (mnp, p1);
        bias.m.x = bias.m.y = 0.0;
        setComply (mnp, (ALONG_Z|ABOUT_X|ABOUT_Y), &bias);
        move (mnp, p2);
        setComply (mnp, ALONG_Z, &bias);
        move (mnp, p3);
        setComply (mnp, 0, &bias);
\end{verbatim}

Since compliance specifications are ``sticky'' ({\it i.e.}, they remain
in effect for more than one motion request), it is useful to have a
routine available that reads back the current settings so that intermediate
specifications can be made without disturbing the overall context. The
routine
\begin{verbatim}
        getComply (mnp, mask, bias)
          MANIP *mnp;
          unsigned long *mask;
          FORCE *bias;
\end{verbatim}
returns the current compliance settings. In the following example,
we specify compliance along the $x$ and $y$ axes for two motions,
without any knowledge of the surrounding context:
\begin{verbatim}
        FORCE bias;
        FORCE oldbias;
        unsigned long oldmask;

        ...

        getComply (mnp, &oldmask, &oldbias);
        bias = oldbias;
        bias.t.x = bias.t.y = 0.0;
        setComply (mnp, (oldmask|ALONG_X|ALONG_Y), &bias);
        move (mnp, p1);
        move (mnp, p2);
        setComply (mnp, oldmask, &oldbias);

        ...
\end{verbatim}

{\tt getComply()} returns the comply state currently set at the
planning level. On the other hand,
a function responsible for implementing compliance at the control
level will need to know the specification in effect for the current
motion. This is obtained with the routine {\tt getActiveComply()}:
\begin{verbatim}
        getActiveComply (mnp, mask, bias)
          MANIP *mnp;
          unsigned long *mask;
          FORCE *bias;
\end{verbatim}

To implement compliant motion in RCCL, the applications generally use some
variation of the position accommodation technique described in [Maples and
Becker 1986]. With this paradigm, correctional displacements responding to
the sensed forces
are added to some coordinate frame in the manipulator's
target position. In effect, the manipulator is made to ``track'' the
observed force errors. Because the lowest level of control is still
a position servo (responding to positional setpoints output by the trajectory
generator) the response of such a system to changes in force
values is somewhat damped.
This, however, is usually desirable, as force control systems
can otherwise be quite unstable.

It is easy to see that accommodation-based compliant control can be implemented
by a functional transform that has access to force sensor data.
Let the compliance specification be given by a selection
matrix ${\bf S}$ and a set of bias forces ${\bf f}_c$, let the
observed forces (in the comply frame)
be ${\bf f}_o$, let the corresponding error
be ${\bf f}_e$, let the value of the comply
transform be ${\bf C}$, and let $p$ be a gain associated with the force
error. Then the computation done by the compliance function
is effectively
\[
        {\bf f}_e \;=\; {\bf S} \; ({\bf f}_c - {\bf f}_o)
\]
\[
        {\bf C} \;=\; {\bf C} \; \hbox{\tt diffToTrsf}( p {\bf f}_e)
\]
The term $p {\bf f}_e$ represents a small Cartesian displacement, which is
converted into a transform using {\tt diffToTrsf()}, and then accumulated in
${\bf C}$. This computation will be performed in the following example
program. 

The force control law presented above is a simple proportional one;
more elaborate control laws can be developed with characteristics
suitable for different sorts of tasks.

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

This program uses RCCL primitives to implement a very simple
compliance package, which is then used
to put the manipulator into a ``free'' mode created
by zero-bias
compliance along each of the three coordinate axes.
In this mode, the arm will
allow itself to be ``pushed around'' in any translational
direction.
The force sensor connected to the robot is the usual 6 DOF wrist
model manufactured
by the Lord Corporation.  The values read back from it are available
in the {\tt forceInput} field of the {\tt HOW} structure.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/comply.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/complyCtrl.560.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot
and lives at {\tt comply.560.c} and {\tt complyCtrl.560.c}
in {\filefont \$RCCL/demo.rccl}.
No equivalent program has been written for the PUMA 260 because
no force sensor is currently available (force sensors tend to be too big for
the PUMA 260).

}
\medskip

The comply package implemented in this program
consists of two planning level routines, {\tt complyInit()} and
{\tt complyRun()}, and two control level routines,
{\tt computeForce()} and {\tt complyFxn()}.

{\tt complyInit()} should be called after the {\tt mnp} structure
for the robot has been created (\cref{5}). It allocates a comply
transform and sets up an internal data structure used
to communicate with the control level functions. It also
sets the translational
and rotational compliance gains, which it takes as arguments.
At (\cref{1}), the comply transform (which will be used to accumulate
the necessary force accommodations) is allocated.
Communication with the control level routines will be done
using a memory object of the type {\tt COMPLY_CTRL_BLK} (defined
in {\tt "manex.560.h"}). This type
is defined as follows:
\begin{verbatim}
        typedef struct {
                float tgain;
                float rgain;
                TRSF *comply;
                MANIP *mnp;
                FORCE force;
                int valid;
                int init;
          } COMPLY_CTRL_BLK;
\end{verbatim}
The fields include the translational and
rotational accommodation gains {\tt tgain} and
{\tt rgain}; {\tt comply},
which is the planning level address of the comply transform and is
used by the control level function to get a valid
pointer to it; {\it force}, which contains force sensor values
transformed into the comply frame;
{\it valid}, a boolean value stating whether or not the force
values are valid for the current control cycle; and
{\it init}, another boolean value used to
initialize the force computation function.
{\tt complyInit()} sets
the fields {\tt tgain} and {\tt rgain} to the gain values
passed in as arguments (\cref{2}), sets the {\tt comply} field to the
comply transform address, and uses {\tt transMotionEval()}
to bind the comply transform to
the function {\tt complyFxn()} on a per-motion basis (\cref{3}).
The routine returns a pointer to the comply control block, which will be
passed back as an argument to {\tt complyRun()}.

The gains which are passed to {\tt complyInit()} are very application
specific. They depend, in general, on the
characteristics of the robot and its position servos, the task
being performed, and the trajectory control rate.

Having allocated a comply transform by calling {\tt complyInit()},
the main program needs to incorporate it
into the target position of
any motion which is to respond to comply requests.
In particular, the comply transform should be the
last transform in the target position's TOOL
frame. In the current program example, this is done for the position
{\tt p1}, defined
as follows:
\[
{\bf T6} \;\; {\bf E} \;\; {\bf COMPLY} \;=\; {\bf B}
\]
where {\bf E} is the end-effector transform,
{\bf B} is a convenient starting location
in space, and {\bf COMPLY} is the comply transform.

The program starts the trajectory generator and calls {\tt complyRun()}
(\cref{6}), which starts up a permanent monitor function
to compute the force values in the current manipulator TOOL frame
(\cref{4}). {\tt complyRun()} sets
the {\tt init} field of the comply control block to tell the monitor
to turn on the force torque sensor.

Compliant motions may now be requested using the system
{\tt comply()} primitive and motions to target positions which contain
the comply transform. The desired ``free manipulator'' effect is
achieved by requesting a zero-force compliance along all three
translational degrees of freedom (\cref{7}). The compliant motion itself is
set to have an indefinite time limit.
While it is in progress,
the program occupies itself by printing out the force sensor values
computed by the monitor function. The monitor writes the force values
into the {\tt force} field of the comply control block for the planning
level to read out. To guarantee consistency of the force data,
the field is written and read using {\tt accessMem()}
(\cref{9}), although (in this case) this is done more for show than
from necessity.

Two control level functions are used to implement the compliance:
{\tt computeForce()}, which computes the forces as seen
in the compliance frame, and {\tt complyFxn()}, which uses these
to determine the necessary changes to the {\bf COMPLY} transform.
Partitioning the computation in this way is reasonable (though not
essential).

{\tt computeForce()} runs all the time, computing force values for
anyone who cares to read them.
It begins by using its application argument to get a pointer to
the comply control block (\cref{10}).
The force sensor readings themselves are obtained from the manipulator's
{\tt HOW} structure (see section \ref{how}).

If {\tt cbk->init} has been
set by the planning level, this indicates that things
are just starting up and the RCI interface
must be told to start reading the force torque
sensor. This is done with a call to the
macro {\tt SET_FORCE_READ()} (\cref{12}), after which {\tt init} is cleared
and {\tt computeForce()} returns, since valid force torque data will not
be ready for at least another control cycle.

To determine whether the {\tt HOW} structure
contains valid force/torque values
for the current control cycle, the field {\tt how->forceOK} is examined
(\cref{13}), and if true, the force values are read out of the {\tt forceInput}
field
and converted from sensor units into Newtons and Newton-millimeters.
(The conversion values in this program are specific to the 6 DOF
Lord sensor.)  Once the force values have been read
in, they still must be converted to the coordinate frame in which the {\bf
COMPLY} transform is being computed. The transform {\bf STOC}
is defined to
map from the wrist sensor frame to the COMPLY frame.
Recall that the application code was requested to make
{\bf COMPLY} the last transform in the {\bf TOOL}
part of the target position. If the rest of {\bf TOOL} is described
by {\bf T}, then we have
\[
{\bf T} \;\; {\bf COMPLY} \;=\; {\bf TOOL}
\]
If we have another transform {\bf F} which maps from the ${\rm T6}$ frame to
the sensor, then {\bf STOC} is given by
\[
{\bf STOC} \;=\; {\bf F}^{-1} \; {\bf T}
\]
which can be computed as
\[
{\bf STOC} \;=\; {\bf F}^{-1} \; {\bf TOOL} \;\; {\bf COMPLY}^{-1}
\]
since {\bf TOOL} is constantly maintained by the
trajectory generator and is available as the
{\tt tool} field in the {\tt MANIP} structure. {\bf STOC} is computed
at location (\cref{14}). Since {\bf F} is simply an offset
along $z$ with no
rotational component, some computation can be saved by simply subtracting this
offset from ${\bf TOOL} \;\; {\bf COMPLY}^{-1}$ rather than premultiplying by
${\bf F}^{-1}$. Once we have {\bf STOC}, it is
used by {\tt transForce()} to map forces
from the sensor frame into the comply frame. These values are
then written into the comply control block
and the {\tt valid} field is set (\cref{15}). For any control cycle in which
no valid force values are available, the {\tt valid} field is cleared
(\cref{16}).

The function {\tt complyFxn()} executes only when the target motion position
contains the {\bf COMPLY} transform. Its job is to use the force values
computed by {\tt computeForce()} and perturb {\bf COMPLY} to
accommodate the error between these values and desired force bias for whatever
degrees of freedom happen to be in compliance mode. Again, the function
uses its application argument to get a pointer
to the comply control block.  It immediately checks the {\tt valid}
field to see if there is proper force data for this cycle, and if not,
then the function bumps a counter and returns. When the counter exceeds a
certain value, it means that force data values have been unavailable for
several consecutive cycles, and the program is aborted (\cref{17}).

The next thing the routine does is use {\tt getActiveComply()} to obtain the
current comply mask and bias forces. The force error is initialized to zero,
and then for each compliant DOF given by the mask,
the corresponding field is set to the difference between the desired
bias force and the observed force (\cref{19}). The required accommodations
are determined by multiplying the force errors by the appropriate gain
to get a displacment. This displacement is small enough to treat as a
differential\footnote{if it is not, then the gains are too high
and there will be stability problems.}, so it can be converted to a
transform using {\tt diffToTrsf()} (\cref{20}). Successive perturbations
are accumulated into {\bf COMPLY} by post multiplication (\cref{21}).

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

This demo program was written at the Jet Propulsion Laboratory on a
PUMA 560 robot. It uses guarded force-limit motions and compliance to locate a
small cylinder and then comply around the outside of it.
It assumes that the robot is fitted with a force/torque sensor and a
peg-like end-effector,
and that there is a small cylinder in the work space which the
operator can position the robot near using the teach routine.

\subsubsection*{planning level module}
\cverbatimfile{exampleCode/cylin.560.c}
\subsubsection*{control level module}
\cverbatimfile{exampleCode/cylinCtrl.560.c}
{\it
\rightskip=0ptplus1fil
NOTE -- this example has been coded for the PUMA 560 robot
and lives at {\tt comply.560.c} and {\tt complyCtrl.560.c}
in the directory {\filefont \$RCCL/demo.rccl}.
An equivalent program for the PUMA 260 lives
at {\tt cylin.260.c} and {\tt cylinCtrl.260.c}, except
that it only simulates the presence of the cylinder
and does not do any real compliance.

}
\medskip

This program makes use of the compliance package defined
in the previous example. Compliance is enabled by calling
{\tt complyInit()} and {\tt complyRun()} after the calls
to {\tt rcclCreate()} and {\tt rcclStart()}, respectively (\cref{1},
\cref{2}).
The operator is instructed to move the robot so that the peg tip is
positioned several millimeters above the cylinder, with the
peg facing down. To facilitate
this, the program enters the teach routine (\cref{3}). When the teach routine
returns, the transform {\bf START} is instantiated so that the position
equation {\tt seek} defines the current robot location (\cref{4}).

The program is now ready to begin the guarded motions to
locate the cylinder exactly. A permanent monitor
function, {\tt forceMonitor()}, is set up to detect force limits (\cref{5}).
This monitor will use the force values calculated by the
monitor {\tt computeForce()} (see the previous example),
which was set running by the routine {\tt complyRun()}.
It is assumed that the peg tip
is located roughly above the center of the cylinder. The exact location
of the center is determined using three guarded moves, in which the robot
moves
away from cylinder, down, and then back towards it in hope of making
contact. Each move is spaced radially $120^\circ$ apart. The different
via points used to do the moves are created by making local modifications
to the transform {\bf OFFSET} in {\tt seek}.

{\tt forceMonitor()} checks force limits independently along or about
each specified axis. For best accuracy, therefore, the force limit
for each guarded move should
lie along a single axis (the $x$ axis is used in this case)\footnote{Alternatively, the monitor function could be implemented
to interpret its limit information differently.}.
How may this be done when the motions themselves approach the
cylinder from three different directions? The answer is to rotate the tool
frame itself between moves so that its $x$ axis is always parallel
to the line of approach. This is the purpose of the transform
{\bf ROTZ}. The reason this works is that the force values themselves are
computed by the
function {\tt computeForce()}
and hence are described with respect to the current tool
frame (offset by the {\bf COMPLY} transform, but {\bf COMPLY}
is the identity at this point in the program, and so
we do not worry about it).

To keep from having to actually rotate the end-effector when the TOOL
frame is rotated, another {\bf ROTZ} is placed on the other side of the
position equation to cancel out the net rotation (the transform
{\bf OFFSET}, which lies in between, does not interfere
because it contains only translations).

\begin{figure}[tbh]
%\rect 3.5truein by 5truein%
\boxedsizedfig{3.5truein}{5truein}%
{\psfig{figure=figs/cylinfig.eps}}%
{The three guarded motions used to locate the
cylinder.}%
\label{cylinfig}%
\end{figure}


Each guarded motion starts at a position
located {\tt MAX_RADIUS} millimeters away from the presumed cylinder center.
This location is specified by setting the $x$ coordinate of {\tt offset} to
{\tt MAX_RADIUS} (\cref{6}). This works for
all the guarded motions because the OFFSET frame is constantly rotated so as
to be parallel to the approach axis (\cref{7}).
The motion sequence consists of
moving out along the radius (\cref{8}), dropping down (\cref{9}), and
coming back in with a force limit set along the $x$ axis
(\cref{12}) (see figure \ref{cylinfig}). The velocity is set low to prevent
damage on contact (\cref{10}), 
and {\tt updateTrans()} is used to request that the transform {\tt touch} be
evaluated when the guarded move terminates (\cref{11}).
Upon contact,
the robot moves back out (\cref{13}) and up (\cref{14}).
The program waits for each guarded move to complete, checks the
associated motion stop
code to make sure that contact was actually achieved, and then uses the
transform {\tt touch} to determine the location of the contact point
in the $xy$ plane of the frame START.

When the guarded moves are complete, the collected $x$ and $y$ coordinates of
the three contact points are used to compute the real center of the cylinder
(\cref{16}) (the function {\tt computeCenter()} is defined in a companion
file).
The {\bf START} transform is then updated correspondingly (\cref{17}).

The last part of the program involves moving the manipulator back to the
cylinder and complying around it. The comply motions
are done with the manipulator in ``tracking mode''
(\cref{18}) (section
\ref{modes}). This ensures that if a delay occurs between
adjacent compliant motions, such that the following one is not on the
motion queue when the previous one ends, the robot will continue
to track the last target (and hence remain in comply mode),
rather than entering the idle state and ``freezing'' where it is.
As with the force limits, the compliance is specified along the $x$ axis
only. For the approach move, during which contact is made,
a small bias force is set (\cref{19}); for the next move, which
moves the peg
around the cylinder (\cref{21}), a larger bias force is used.
The action of moving the peg around the cylinder is achieved by binding
{\bf ROTZ} to a rotation function (\cref{20}). When the circular motion has
finished, the program waits for another 10 seconds (\cref{22}) to
illustrate the effect of tracking mode:
the rotation around the cylinder should continue during this time.
Finally, the arm is moved back up to its starting position
(\cref{23}).

The control level module for this program contains the
rotation function {\tt rotzFxn()} used for the compliant move
and the force monitor function {\tt forceMonitor()}. It also
loads in
the routines {\tt complyFxn()} and {\tt computeForce()},
which belong to the comply package.
The force limit monitor uses the force data computed by the
comply routine {\tt computeForce()}.
It gets the force limit specification for the
current motion (\cref{24}), checks to see that the force data is valid
(\cref{25}), and cancels the motion if the observed forces exceed any of the
specified limit values (\cref{26}).

\newpage
\done
