%%% Mention something about lag possibility when one manip's
%%%  equation depends on the solution of another's
\input rccluginit.latex

\chapter{Multi-Robot Capabilities}

\section{Controlling Multiple Robots}

Multi-RCCL allows several robots to be controlled from within one
program. All the programmer needs to do is make separate calls
to {\tt rcclCreate()} for each robot, and then
refer to the different {\tt MANIP} structures as required.
At the Jet Propulsion Laboratory, there are two robots called
{\tt "Right"} and {\tt "Left"}. A piece of code which
moves both of them to their starting positions
looks like this:
\begin{verbatim}
        MANIP *right, *left;
        JNTS parkR, parkL;

        ...

        getRobotPosition (parkL.v, "rcclpark", "Right");
        getRobotPosition (parkL.v, "rcclpark", "Left");

        right = rcclCreate ("Right", 0);
        left  = rcclCreate ("Left", 0);

        rcclStart();

        movej (right, &parkR);
        movej (left, &parkR);

        waitForCompleted (right);
        waitForCompleted (left);

        rcclRelease (1);
\end{verbatim}

The only real limitation to this capability is the
CPU power available to do all the necessary trajectory computations.
To this end, the multi-CPU versions of the system are quite useful.
The trajectory generation task for each robot can be assigned to
a different CPU.
{\tt rcclCreate()} tries to do this by default, starting with
the auxiliary CPUs.
If for some reason the application wants to have
the trajectory task for a robot run on a particular CPU,
then this can be specified explicitly using the CPU selection mask
(the second argument to {\tt rcclCreate()}). In the example above,
\begin{verbatim}
        right = rcclCreate ("Right", 0x2);
        left  = rcclCreate ("Left", 0x2);
\end{verbatim}
would explicitly assign both robots to CPU 1 (the CPUs are numbered starting
at 0, where 0 is the arbiter CPU).

\section{Virtual Manipulators}\label{virtual}

Another, particularly useful capability of Multi-RCCL is the ability to create
``virtual manipulators''. These are basically ${\rm T6}$ frames without the
attached robot.
They are controlled by the same data structures and routines as
normal RCCL robots, are assigned to trajectory task on
a particular CPU, and can be moved with the usual motion primitives.
Their only restriction is that joint level features are
not defined for them; {\it i.e.}, calling
{\tt setJointVel()} or doing {\tt setMod('j')} on a virtual manipulator
will cause an error.

A virtual manipulator is created with a call to {\tt rcclCreate()}.
The system will set up a virtual manipulator if the name
of the robot (given by the first argument) is not equal to the name of any
real robot configured into the system.
Naturally, the name {\tt foo} is
a common one:
\begin{verbatim}
        vmnp = rcclCreate ("foo", 0);
\end{verbatim}
The {\tt t6} (and related fields) of the
{\tt MANIP} structure are initialized to the identity. The
joint angle structures, such as {\tt j6} and the like, are simply
zeroed and left that way. The only interpolation mode allowed for a
virtual manipulator is Cartesian (which of course means that unlike
manipulators corresponding to real robots, the default interpolation
mode for virtual manipulators is Cartesian).

Because a virtual manipulator has no physical constraints, it
can be assigned any acceleration or velocity limit
desired. When created, it is
given the same default Cartesian velocity limits as the rest of the
manipulators in the system, along with the acceleration limits
{\tt DEFAULT_TRANS_ACCEL} and {\tt DEFAULT_ROT_ACCEL}.

Sometimes, when moving a virtual manipulator to a position, it is
nice to simply ``put it there'', rather than having to move it
there with a {\tt move()} command. Again, this is possible because there
are no physical constraints on the manipulator. The primitive
\begin{verbatim}
        maintain (vmnp, pos)
          MANIP *vmnp;
          POS *pos;
\end{verbatim}
is designed to do this: it ``puts'' {\tt vmnp} at the
target position {\tt pos} and keeps it there indefinitely,
until another move or maintain request is received.

One place where virtual manipulators are useful is in system testing,
since there is no robot to worry about or even to start up.
As a simple example, we present the following
program based on one of the internal RCCL testing
routines. The program tests
to see that the system is
behaving correctly with regard to the {\tt setCartVel()}
primitive. To do so, it reads back the number of control cycles
assigned by the system to a particular motion (the $\sigma$ value; see section
\ref{motions}) and checks that this corresponds to the specified
velocity.
The check count is much easier to determine if the transition time is
set to 0, which is risky on a real robot but completely natural
on a virtual one.
\begin{verbatim}
        MANIP *vmnp;
        TRSF *target;
        POS *p0;
        int sigmaCheck;
        int sigma;

        target = allocTrans (NULL, UNDEF);
        p0 = makePosition (NULL, T6, EQ, target, TL, T6);

        vmnp = rcclCreate("foo", 0);
        rcclStart();

        target->p.z = 123.0;
        setCartVel (vmnp, 123.0, F_UNDEF);
        setTime (vmnp, 0.0, F_DEFAULT);
        move (vmnp, p0);
        rcclBlock();
        getActiveMotionCounts (vmnp, NULL, &sigma);
        sigmaCheck = NINT(1000.0/rcclGetInterval());
        if (sigma != sigmaCheck)
         { printf ("sigma = %d vs. %d\n", sigma, sigmaCheck);
         }
\end{verbatim}
The virtual manipulator is told to move from its initial
(identity) position to a target position that has a displacement of
123 along the $z$ axis. Since the translational velocity is also set
explicitly to 123, and the transition time is set to 0, the time required for
this motion should be 1 second. This is verified
by reading back the actual motion segment count $\sigma$
with the routine {\tt getActiveMotionCounts()}. The preceding
call to {\tt rcclBlock()} ensures that at least one control cycle
has elapsed
since the move request and that $\sigma$ has in fact been computed.

Another, more application oriented use of the virtual manipulator is
in controlling the trajectory of a robot arm by making it track a
remote frame.
Suppose that a robot target position {\tt p0} is composed of the
usual ${\rm T6}$, E, and Z (base) frames, plus another frame OBJ defined
with respect to Z:
\[
{\bf Z} \;\; {\bf T6} \;\; {\bf E} \;=\; {\bf OBJ}
\]
Now suppose that OBJ happens to be the ${\rm T6}$ frame for a virtual
manipulator. If this virtual manipulator is moved to, or made to
follow, some arbitrary target position {\tt pv}, defined (generally) as
as
\[
{\bf OBJ} \;=\; {\bf TARGET},
\]
then we will have the kinematic
situation seen in figure \ref{vloop}.

\begin{figure}[tbh]
%\rect 5truein by 3truein%
\boxedsizedfig{5truein}{3truein}%
{\psfig{figure=figs/vloop.idr}}%
{Kinematic graph in which a robot's target position
is bound to a virtual manipulator.}%
\label{vloop}%
\end{figure}
%

Any motion of the virtual manipulator will be tracked by the real
manipulator. If the motion of the real robot
to {\tt p0} is set for indefinite duration, then
an entire set of motions can be specified for it by moving only
the virtual manipulator.
\begin{verbatim}
        setTime (mnp, F_DEFAULT, F_UNDEF);
        move (mnp, p0);

        ... now control 'mnp' using 'vmnp' ...

        move (vmnp, p1);
        stop (vmnp, 1000.0);
        move (vmnp, p2);

        ... and now release 'mnp' ...

        waitForCompleted (vmnp);
        stopCurrentMotion (mnp);
\end{verbatim}
These induced motions will differ from direct motions on {\tt mnp}
because the coordinate frame in which the drive parameters are computed will
be the TOOL frame of {\tt vmnp}, not {\tt mnp}. This can in fact be useful if
it is necessary to move a robot with the
drive transform
computed somewhere other than adjacent to the TOOL frame.

The transition and path segment times for induced motions,
unless explicitly stated, will be based on the default acceleration
and velocity limits for the virtual manipulator rather than the real
manipulator. Because of this, it may be desirable to set these
limits on the virtual manipulator to be equal to the
limits for the real manipulator.
A piece of code that does this is
\begin{verbatim}
        float tval, rval;

        getCartVel (mnp, &tval, &rval);
        setCartVel (vmnp, tval, rval);
        getCartAccel (mnp, &tval, &rval);
        setCartAccel (vmnp, tval, rval);
\end{verbatim}
where the velocity and acceleration scale factors are assumed to
be 1; if not, they can be ``brought over'' as well:
\begin{verbatim}
        setSpeed (vmnp, getSpeed (mnp));
        setAccelScale (vmnp, getAccelScale (mnp));
\end{verbatim}

It is possible not only to drive a real manipulator with a virtual one,
but to connect two real manipulators together in a ``master-slave''
configuration. All we need to do is to create a ``slave position'' for the
slave robot which explicitly contains the {\bf T6} transform for the
master robot.
For example, consider the following:
\begin{verbatim}
        MANIP *master, *slave;
        POS *ps;

        master = rcclCreate ("Right", 0);
        slave  = rcclCreate ("Left", 0);

        ... set up appropriate transforms 'a' and 'b'
            connecting the slave to the master at
            the base and tool tips ...

        ps = makePosition (NULL, a, T6, b, EQ, master->t6);

        setTime (slave, F_DEFAULT, F_UNDEF);
        move (slave, ps);

        ... slave is now bound to master ...
\end{verbatim}
After this code is executed, the robot ``Left'' is slaved to the robot
``Right'' for all subsequent motions (until the move to {\tt ps}
is canceled). The kinematic situation is essentially the same as
in figure \ref{vloop}, except that {\bf Z} is replaced
by {\bf A} and {\bf E} by {\bf B}.

\begin{figure}[tbhp]
%\rect 5truein by 3truein%
\boxedsizedfig{5truein}{3truein}%
{\psfig{figure=figs/msloop.idr}}%
{Kinematic graph for a simple master-slave
robot configuration.}%
\label{msloop}%
\end{figure}

\begin{figure}[tbhp]
%\rect 5truein by 3.5truein%
\boxedsizedfig{5truein}{3.5truein}%
{\psfig{figure=figs/object.idr}}%
{Kinematic graph for two manipulators holding
a common object.}%
\label{object}%
\end{figure}

When manipulators are connected together and made to move around,
a problem is usually created by the residual forces which build up
between them: they start doing ``isometric exercises'' with one another.
In general, this can be solved by inserting a {\bf COMPLY} transform at some
point in the kinematic chain between the two robots. The {\bf COMPLY}
transform is
attached to a real-time function that adjusts its values
to cancel out the residual forces. While this transform can be placed
anywhere between the two robots, it is desirable in practice to place
it as close as possible to the place where the forces are
actually sensed. If they are sensed in or near the {\bf T6} frame of the
slave manipulator, then we will want to construct a slave kinematic loop
like the one in figure \ref{msloop}.

It is also possible to slave two or more real robots to a single
master manipulator, which may be either a virtual manipulator or
another real robot. There should be at least one {\bf COMPLY} transform
for every extra real manipulator in the graph.
If the master manipulator is a virtual manipulator, then
we have a kinematic situation which is useful
if we want two or more real robots to
manipulate a common physical object. This situation is
shown in figure \ref{object}.

Each manipulator is ``connected'' to the physical object using
a position equation that looks like
\[
{\bf A} \;\; {\bf T6} \;\; {\bf E} \;\; {\bf COMPLY} \;=\; {\bf OBJ} \;\; {\bf G}
\]
Transform {\bf A} maps from the object base frame to the manipulator base
frame;
{\bf E} is the transform from {\bf T6} to the point at which the object is
grasped;
{\bf OBJ} is location of the object (equal to
the ``{\bf T6}'' of the virtual
manipulator controlling the object); and
{\bf G} transforms from the object location
to the grasp point. On the assumption that the force sensors
are located near the grasp points, the {\bf COMPLY} transforms
are placed there as well. This example uses two comply transforms, which is
not strictly necessary since there are only two real manipulators.
However, using two transforms potentially allows us to ``balance''
the interactions between the two robots and achieve true distributed
load sharing.

No attempt will be made to discuss here the ways in which the comply
transforms can be computed to achieve cooperative control between
manipulators, as this is still an active area of research.
In particular, the RCCL site at the Jet Propulsion Laboratory
is studying problems of this sort.

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

A virtual manipulator which is used to drive a slave manipulator
is sometimes referred to as an {\it object} frame.
This program demonstrates two manipulators tracking a single object
frame.
Because the program runs in free space with no kinematic connection
between the two arms, there is no need for any of the {\bf COMPLY}
transforms discussed above.
A diagram of the kinematic relationships is given in figure \ref{trackII}.
The object frame is made to trace sequences of boxes and circles, with
both attached robots duplicating these motions.

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

}
\medskip

\begin{figure}[tbh]
%\rect 5truein by 3.5truein%
\boxedsizedfig{5truein}{3.5truein}%
{\psfig{figure=figs/trackII.idr}}%
{Kinematic diagram for the program ``trackII''.}%
\label{trackII}%
\end{figure}
%

The names of the two robots are ``Rsun'' and ``Lsun'' (\cref{1}).
The name of the object frame, in keeping with tradition, is ``foo''
(\cref{2}). The position {\tt mnpPos}, defined as
\[
{\bf T6} \;\; {\bf E} \;=\; {\bf OFFSET} \;\; {\bf OBJ}
\]
connects each manipulator to the object frame (\cref{3}).
{\bf OFFSET} is set to the initial value of ${\rm T6} \;\; {\rm E}$.
The object frame itself is moved either with relative motions specified
with respect to {\tt obj->last}, or by changing the value of
{\bf TRACK} in the simple target
position {\tt objPos}:
\[
{\bf T6} \;=\; {\bf TRACK}
\]

After the trajectory generator is started, both robots are moved to their
usual starting positions. The {\tt OFFSET} transform in
{\tt mnpPos} is then updated for each of them, and both
are bound to position {\tt mnpPos} indefinitely (\cref{4}). Before each motion
request is issued, the transform {\tt offset} is instantiated for each robot
so that {\tt mnpPos} defines its current location.
The velocity and acceleration limits for the object frame are then
set equal to those for the first manipulator (\cref{5}) (both robots are
assumed to be of the same type).

The program next executes a loop in which the object frame is made to first
trace out a box, and then a circle. The ``box'' motion is accomplished with a
series of relative motions (\cref{6}).
The ``circle'' motion
is done by binding the transform {\bf TRACK} to a
circle tracing function (\cref{7}). The last motion in the loop is a move
to {\tt obj->park}, which ensures that the next position of the object frame
does not drift from one iteration of the loop to the next (\cref{8})
(the park location for virtual manipulators is the identity
transform).

\newpage
\done
