\input rccluginit.latex

\chapter{Describing Positions in Space}

Describing a manipulator task typically requires
specifying positions to
be reached in space (the {\it where}) as well as specifying aspects of the
trajectory (the {\it how}).
RCCL describes target positions using either Cartesian position equations
or sets of joint angles. This chapter describes the former.

\section{Position Equations}\label{poseqns}
%\index {position equations}{}

Position equations remove the need for absolute
reference coordinates; they are also
one representation of the more general concept
of transformation graphs.
The position relationships of a set of coordinate frames ${\rm F}_i$, 
$i=1,\dots,n$,
can be expressed
in terms of transformation products.
Let the transformation matrix ${\bf T}_i$
describe the position of the frame ${\rm F}_{i+1}$ relative
to the frame ${\rm F}_i$, with
${\bf T}_n$ describing the transformation from frame 
${\rm F}_n$ to ${\rm F}_1$.
This is represented by the equation
\[
        {\bf T}_1\;{\bf T}_2\;\ldots\;{\bf T}_n\;=\;{\bf I}
\]
where {\bf I} is the identity matrix.
A closed path of transformations from frame ${\rm F}_1$
back to frame ${\rm F}_1$, via the frames ${\rm F}_i$, $i=2,\dots,n$,
describes the position of ${\rm F}_1$ with respect to
itself.
The situation can be illustrated using a directed closed graph,
as shown in figure \ref{graphA},
where the vertices are frames and the arcs are transforms.

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/graphA.idr}}%
{A directed closed transform graph.}%
\label{graphA}%
\end{figure}


Given a set of frames, containing two frames A and B,
we can always find more than
one path connecting A to B.
Denoting the frames on one path by ${\rm F}_i$, $i=0,\dots,n$, and
the frames on the other path by ${\rm G}_i$, $i=0,\dots,m$, we show such
a graph in figure \ref{graphB}. 

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/graphB.idr}}%
{Transform graph containing two distinct paths between frames A and B.}%
\label{graphB}%
\end{figure}

The corresponding transformation equation is:
\[
{\bf T}_0\;{\bf T}_1\;{\bf T}_2\;\ldots\;{\bf T}_n\;=
\;{\bf R}_0\;{\bf R}_2\;\ldots\;{\bf R}_m
\]
Closed transformation graphs can be expressed in terms of a set
of transformation equations.

RCCL uses transformation equations
as one means of describing positions
the manipulator has to reach.
These positions always include the special manipulator
transform {\bf T6}, which we will describe next.

Assume that we have a manipulator with 6 links, labeled from 1 to 6,
and a base link, labeled 0.
Each of the manipulator links is assigned a frame ${\rm A}_i$,
and the transform matrix ${\bf A}_i$
describes its position with respect to the previous link's
frame as a function of
the joint variable. 
The position of link 1 is described with respect to the base.
The transformation product
\[
{\bf T6}\;=\;{\bf A}_1\;\ldots\;{\bf A}_6
\]
describes the position of the last link with respect to the base.
Current convention is to define the transformations ${\bf A}_i$
according to the procedure specified by Denavit and Hartenberg 
[Lee 1982]. This generally will not place the last frame, ${\rm A}_6$,
directly at the end effector. Instead,
to ``reach'' the end effector frame, it
is usually necessary to provide an additional {\it tool} transform.

\section{Using Position Equations to Describe Motion Targets}\label{motiontarget}

Let us now set up a position equation which
describes a situation where the manipulator is to grasp an object
lying on a table.
First we must assign frames to each of the elements involved:

%%% CN wants a diagram of this naming scheme.  I must admit it would be
%%%  a bit of a help.
\begin{list}{}{}
\bigskip
\item [$\bullet$]
Frame B is assigned to the manipulator base.

\bigskip
\item [$\bullet$]
Frame M is located in the last link of the manipulator.

\bigskip
\item [$\bullet$]
A tool is attached to link 6 and the frame T is assigned
to its tip.

\bigskip
\item [$\bullet$]
Frame W describes the position of the work table.

\bigskip
\item [$\bullet$]
The position of an object lying on the table is described by frame O.

\bigskip
\item [$\bullet$]
A grasp position is described by the frame G.
\end{list}

Suppose that the manipulator is moving so as to grasp the object.
The associated frames can be grouped together into the graph
shown in figure \ref{graphC}.

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/graphC.idr}}%
{Example frames for a manipulator grasp task.}%
\label{graphC}%
\end{figure}

To obtain a transformation equation from this graph,
the arcs are oriented and labeled with transforms.
The choice is arbitrary but a reasonable selection is shown in figure 
\ref{graphD},
where {\bf END}, {\bf BASE}, {\bf GRASP}, and
{\bf OBJ} are predetermined transforms, and
{\bf T6} connects the manipulator base to link 6.
The transform {\bf DRIVE}, which will be discussed 
in more detail below, relates the current position of the tool tip to its
final desired position. A diagram of where all these frames
might actually be located in the workspace is given in figure
\ref{graspgraph}. 

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/graphD.idr}}%
{Example frames, plus transforms, for a manipulator grasp task.}%
\label{graphD}%
\end{figure}


\begin{figure}[tbh]
%\rect 5truein by 5truein%
\boxedsizedfig{5truein}{5truein}%
{\psfig{figure=figs/graspgraph.idr}}%
{Example frames for grasp task superimposed on a
diagram of the workspace.}%
\label{graspgraph}%
\end{figure}


In making the manipulator travel to its destination,
it is necessary to vary {\bf T6} from its starting
value to the one defined by the position equation,
which amounts to changing the value of {\bf DRIVE} from its initial
value to the identity matrix. 
{\bf DRIVE} is therefore time-varying, and when appropriate
will be denoted as ${\bf DRIVE}(t)$.
If we define the time at the beginning of the motion to be
0 and the time at the end of the motion to be $\sigma$, then
the kinematic situation 
is described at the beginning of the motion by
\[
{\bf BASE} \;\; {\bf T6}(0) \;\; {\bf END} \;=\;
{\bf OBJ} \;\; {\bf GRASP} \;\; {\bf DRIVE}(0)
\]
and at the end of the motion by
\begin{equation}
{\bf BASE} \;\; {\bf T6}(\sigma) \;\; {\bf END} \;=\;
{\bf OBJ} \;\; {\bf GRASP} \label{final}
\end{equation}
By definition, ${\bf DRIVE}(\sigma) \;=\; {\bf I}$.

The two common methods of achieving
this are {\it joint interpolation} and
{\it Cartesian interpolation}. Joint interpolation involves 
solving for the target value of {\bf T6} ({\it i.e.}, the value with
${\bf DRIVE} = {\bf I}$), solving for the corresponding 
joint values, and then interpolating to these from the
initial joint values. This produces a ``straight line'' motion
in joint coordinates. The transform {\bf DRIVE} changes 
implicitly during this process, but not in a way that is
necessarily obvious. 
On the other hand, Cartesian interpolated motion
is achieved by explicitly modifying {\bf DRIVE} so that it traces a
straight line in Cartesian coordinates, and then solving for
{\bf T6} and the associated joint values during each
control cycle. This produces a ``straight line'' in Cartesian
coordinates.
Rotations are handled by interpolation about one or two
principal axes, as described in [Paul 1981], Chapter 5.
Both forms of motion interpolation are supported by RCCL. 

Transform equations can be rewritten, solved for any of the terms,
or replaced by equivalent ones.
For example, equation \ref{final} can be rewritten as
\[
{\bf BASE}\;\;{\bf T6}\;=\;{\bf OBJ}\;\;{\bf GRASP}\;\;{\bf END}^{-1}
\]
or as
\[
{\bf T6}\;=\;{\bf BASE}^{-1}\;{\bf OBJ}\;\;{\bf GRASP}\;\;{\bf END}^{-1}
\]

Internally, when RCCL is told to move the manipulator
to a given target position, it converts the corresponding 
equation into the canonical form
\begin{equation}
{\bf T6} \;\; {\bf TOOL} \;=\; {\bf COORD} \label{canonical}
\end{equation}
where {\bf TOOL} and {\bf COORD} can be composed of arbitrary
numbers of sub-transforms. For equation \ref{final}, we would
have
\begin{eqnarray*}
{\bf TOOL} & \;=\; & {\bf END} \\
{\bf COORD} & \;=\; & {\bf BASE}^{-1} \; {\bf OBJ} \;\; {\bf GRASP} 
\end{eqnarray*}
This internal representation tells the trajectory
generator where to place the {\bf DRIVE} transform
for Cartesian motions. It is
inserted after the tool component:
\[
{\bf T6} \;\; {\bf TOOL} \;=\; {\bf COORD} \;\; {\bf DRIVE}
\]

More detailed information on how RCCL generates trajectories can be
found in the paper {\it Trajectory Generation in Multi-RCCL}, which is
part of the document set.

\section{Frame and Transform Names}

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/loopC.idr}}%
{Transform graph where some frames have the same
name as one of their incident transforms.}%
\label{loopC}%
\end{figure}


It is often possible to equate the name of a transform and the frame
it maps to without causing confusion. For instance, in figure \ref{loopC},
frames ${\rm T6}$, TOOL, and C are given the same names as the
transforms {\bf T6}, {\bf TOOL}, and {\bf C}. It is also often possible
to dispense with frame names entirely.

When describing program examples, transforms may be referred
to either mathematically (e.g., {\bf T1}) or by the name of their
corresponding variable within the program (e.g., {\tt t1}). A frame
will often be named after one of its incident transforms (e.g.,
T1).

By historical continuity, RCCL uses the name
{\bf T6} (and ${\rm T6}$) to denote
the base-to-last-link transform product (and
associated frame) for any manipulator,
regardless of whether or not it actually has 6 links. 

\section{Defining Position Equations}

Within an RCCL program, position equations like the ones described
above can be defined with the primitive
{\tt makePosition()}.
%\index {\tt makePosition()}{}
This takes a variable number of arguments
which define the equation's name,
left hand side, and right hand side.
The function definition looks like
\begin{verbatim}
        POS *makePosition (name, lhs [, lhs] ..., EQ, 
                                 rhs [, rhs] ..., [, TL, tl] , NULL)
          char *name;
          TRSF_PTR lhs ..., rhs ..., tl;
\end{verbatim}
The first argument is a string naming the position.
This name is optional, and can be declared as {\tt NULL} if desired;
its main purpose is to provide a key for referencing the equation
from some other part of the program (section (\ref{usingobjects})).
The remaining arguments are pointers to the transforms which
comprise the equation. All the specified transforms must have been
allocated using one of the {\tt allocTrans()} routines; this ensures
that the control level can access them when necessary.
To create a position equation, simply list as arguments all the transforms 
forming the left-hand and right-hand sides, separate them
with the predefined variable {\tt EQ}, and terminate the argument
list with {\tt NULL}.
For instance, consider the simple position equation
\[
{\bf A} \; {\bf B} \;=\; {\bf C} \; {\bf D},
\]
which is equivalent to the graph shown in figure \ref{loopB}.

\begin{figure}[tbh]
%\rect 5truein by 2truein%
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/loopB.idr}}%
{A sample position loop.}%
\label{loopB}%
\end{figure}


Assuming that the necessary transforms have been allocated,
and pointers to them are contained in the variables
{\tt a}, {\tt b}, {\tt c}, {\tt d}, 
this can be created with the following call:
\begin{verbatim}
        POS_PTR p;

        p = makePosition ("loop", a, b, EQ, c, d, NULL);
\end{verbatim}
The function allocates a data structure representing the kinematic loop,
gives it a string name {\tt "loop"}, and returns a pointer to
it. The transforms which lie on the left-hand side of the equation
are assumed to be directed {\it clockwise\/}, while the transforms on the
right-hand side are directed {\it counterclockwise\/}.
An arbitrary number of
transform arguments may be specified to
{\tt makePosition()}, although a ``safety limit'' of 100
is imposed at the time of this writing.

What can we now do with this position equation? The principal
thing we can do is solve for any one of its component transforms in terms of
the values of the other transforms. We can also multiply together any
set of transforms which constitutes a sub-chain of the equation. The
routines to do these operations are described in section \ref{compute}.

It should be noted that it is not necessary for the component transforms
of a position equation to be set to any particular value when the
position is defined. This only needs to be done before the position is 
actually used within the program. The equation structure itself
contains only pointers to the component transforms; these pointers are
used to look up the actual values ``on the fly'' when the equation
is referenced.

The above position equation ({\tt "loop"}) cannot be used to specify the
target of a manipulator motion because it does not contain
the transform {\bf T6}. To include
{\bf T6} in an equation definition, the predefined variable
{\tt T6} should be inserted at the appropriate
point into the argument list to {\tt makePosition()}.
Moreover, if we want to specify a TOOL frame that is different
from the ${\rm T6}$ frame, then the predefined variable {\tt TL} should be
placed at the end of the argument list (before the {\tt NULL}) and followed by
a pointer {\tt tl} to a transform denoting this TOOL frame. 
The transform referenced by {\tt tl} must be one of the equation's component
transforms. Sometimes, the {\bf TOOL} component of a position equation is
composed of more than one transform. If this is the case, {\tt tl}
should be the transform adjacent to the TOOL frame itself.
The system will then define the total {\bf TOOL} component to be
the product of all the transforms lying between {\bf T6} (exclusive)
and {\tt tl} (inclusive), in the same direction 
as {\bf T6}.

When a TOOL frame is specified with {\tt TL}, we can omit the {\tt NULL}
at the end of the argument list.

For example, suppose we wish to create the position equation in \ref{final}.
Assume that the transforms {\bf BASE}, {\bf TOOL}, {\bf OBJ}, and 
{\bf GRASP}
have been allocated, and pointers to them are stored
in the variables
{\tt base}, {\tt tool}, {\tt obj}, and {\tt grasp}.
The equation can then be defined by
\begin{verbatim}
        POS_PTR p;

        p = makePosition ("P", base, T6, tool, EQ, obj, grasp, TL, tool);
\end{verbatim}

If an equation definition contains {\tt T6} but does not contain
a {\tt TL} specification, then the TOOL frame will be set equal to the
${\rm T6}$ frame (i.e., ${\bf TOOL} \;=\; {\bf I}$ in (\ref{canonical}));
the same thing can be accomplished by using a {\tt TL} specification
and setting {\tt tl} to either {\tt T6} or {\tt NULL}.
A {\tt TL} specification will be ignored if the equation does not
contain a {\bf T6} transform. 

To illustrate the different ways of setting up the canonical components,
consider the components {\bf COORD} and {\bf TOOL} that correspond to
the following calls to {\tt makePosition()}:
\begin{verbatim}
    makePosition ("P0", T6, EQ, h, NULL); /* produces ... */
\end{verbatim}
\begin{eqnarray*}
{\bf TOOL} & \;=\; & {\bf I} \\
{\bf COORD} & \;=\; &  {\bf H} 
\end{eqnarray*}
\begin{verbatim}
    makePosition ("P1", T6, EQ, h, TL, T6); /* produces ... */
\end{verbatim}
\begin{eqnarray*}
{\bf TOOL} & \;=\; & {\bf I} \\
{\bf COORD} & \;=\; & {\bf H} 
\end{eqnarray*}
\begin{verbatim}
    makePosition ("P2", T6, t, EQ, h, TL, t); /* produces ... */
\end{verbatim}
\begin{eqnarray*}
{\bf TOOL} & \;=\; & {\bf T} \\
{\bf COORD} & \;=\; & {\bf H} 
\end{eqnarray*}
\begin{verbatim}
    makePosition ("P3", T6, a, EQ, h, g, TL, T6); /* produces ... */
\end{verbatim}
\begin{eqnarray*}
{\bf TOOL} & \;=\; & {\bf I} \\
{\bf COORD} & \;=\; & {\bf H} \; {\bf G} \; {\bf A}^{-1}
\end{eqnarray*}
\begin{verbatim}
    makePosition ("P4", T6, t1, t2, EQ, h, g, TL, t2); /* produces ... */
\end{verbatim}
\begin{eqnarray*}
{\bf TOOL} & \;=\; & {\bf T1} \; {\bf T2}\\
{\bf COORD} & \;=\; & {\bf H} \; {\bf G}
\end{eqnarray*}

When a position is no longer needed, it can be deallocated
with the call
\begin{verbatim}
        freePosition(p)
        POS_PTR p;
\end{verbatim}
%\index {\tt freePosition()}{}
%\index {{\tt POS} data structure}{}
Care must be taken that the corresponding position is no longer in use.
The equation's component transforms are not freed; they must be freed
individually using
{\tt freeTrans()}.

\section{Computing with Position Equations}\label{compute}

The most basic use of position equations in RCCL is to use them
directly in {\tt move()} as the target specifications
for motion requests. This will be discussed extensively in the
following chapters.
It is also possible to do direct computations with them.
The following primitives allow a programmer to solve for
a particular transform or sub-chain within an equation:
\begin{verbatim}
        TRSF_PTR solveTrans (tr, p, t1, tx)
          TRSF_PTR tr, t1, tx;
          POS *p;

        TRSF_PTR *solveChain (tr, p, t1, t2, tx)
          TRSF_PTR tr, t1, t2, tx;
          POS *p;
                   
        TRSF_PTR *solveInvChain (tr, p, t1, t2, tx)
          TRSF_PTR tr, t1, t2, tx;
          POS *p; 
\end{verbatim}
{\tt solveTrans()} takes position equation {\tt p} and solves
it for the transform {\tt t1} (which must be an element of
{\tt p}). If {\tt p} contains a {\bf T6} transform, but it is not the
transform being solved for, then the argument
{\tt tx} is used to provide a specific value for {\bf T6}.
Otherwise, {\tt tx} can be specified as {\tt NULL}.
{\tt solveTrans()} returns a pointer to its result argument.

For example, suppose we have the position equation
\begin{equation}
{\bf Z} \;\; {\bf T6} \;\; {\bf E} \;=\; {\bf A} \;\; {\bf B} \;\; {\bf C}
\label{poseq}
\end{equation}
representing the kinematic graph in figure \ref{loopA}.

\begin{figure}[tbh]
%\rect 5truein by 2truein
\boxedsizedfig{5truein}{2truein}%
{\psfig{figure=figs/loopA.idr}}%
{A sample position loop containing the
manipulator transform {\bf T6}.}%
\label{loopA}
\end{figure}

This can be declared with the {\tt makePosition()} call
\begin{verbatim}
        p = makePosition ("p", z, T6, e, EQ, a, b, c, TL, e);
\end{verbatim}

If at some later time we wish to solve for {\bf A}, we can call
\begin{verbatim}
        solveTrans (r, p, a, tx);
\end{verbatim}
which will compute
\[
{\bf A} \;=\; {\bf Z} \;\; {\bf TX} \;\; {\bf E} \;\; {\bf C}^{-1} \; {\bf B}^{-1}
\]
and place the result into {\tt r}. The original value of
{\tt a} is left unchanged.

{\tt solveChain()} and {\tt solveInvChain()} are used to evaluate
sub-sections of a position equation.

{\tt solveChain()} goes clockwise around the kinematic loop 
described by {\tt p} and multiplies together all
transforms found between {\tt t1} and {\tt t2}, inclusive
(recall that the clockwise-directed transforms are those
that appear on the left-hand side of the equation).
If one of the transforms is directed counterclockwise,
then its inverse value is used in the multiplication. Again, values for the
transform {\bf T6} are supplied, if necessary, by the 
argument {\tt tx}. For example, in (\ref{poseq}),
the calls
\begin{verbatim}
        solveChain (r, p, a, c, tx);

        solveInvChain (r, p, T6, c, tx);
\end{verbatim}
would compute 
\[
{\bf R} \;=\; {\bf A}^{-1} \; {\bf Z} \;\; {\bf TX} \;\; {\bf E} \;\; {\bf C}
\]
and
\[
{\bf R} \;=\; {\bf TX}^{-1}\;{\bf Z}^{-1}\;{\bf A}\;\;{\bf B}\;\;{\bf C}^{-1}
\]
respectively.

{\tt solveInvChain()} does the same thing as {\tt solveChain()},
except it goes around the loop counterclockwise.

Kinematic equation loops 
comprise a restricted case of {\it transformation graphs}.
Multi-branch graphs can be useful in modeling situations where 
several time varying coordinate frames are interrelated.
They can be implemented using the primitives described above,
and/or the basic transform multiplication routines.

\begin{figure}[tbh]
%\rect 5truein by 4truein%
\boxedsizedfig{5truein}{4truein}%
{\psfig{figure=figs/coop.idr}}%
{Kinematic diagram for a cooperative 
pick-and-place operation.}%
\label{coop}%
\end{figure}


As a simple example, consider the following: suppose two manipulators
are working together on a task, with manipulator 1 occasionally putting
down an object that
manipulator 2 has to pick up.
Assume that the
relative location of the two robots is described by the transform
{\bf BASE2}, which maps from the base frame of the first robot to
the base frame of the second (see figure \ref{coop}).
Assume also that the first robot is being guided
by real-time sensors, so it is not known in advance exactly where the
object will be put down. Let the ``put down'' spot
be described by the position equation
by 
\begin{equation}
{\bf T6} \;\; {\bf E}_1 \;=\; {\bf PLACE} \label{place}
\end{equation}
where ${\bf E}_1$ is a tool transform and {\bf PLACE} locates the
object's grasp point relative to the manipulator's base.
When the object is actually put down, we can solve for
all the components of (\ref{place}) exactly.
Now, let the ``pick up'' spot for 
the second robot be described by
\begin{equation}
{\bf T6} \;\; {\bf E}_2 \;=\; {\bf PICK} \label{pick}
\end{equation}
Picking the object up requires moving the second robot's TOOL
frame to the
same spatial location that was occupied by the first
robot's TOOL frame. This amounts to finding a value 
for the transform {\bf PICK}.  From figure \ref{coop}, we can see that
there is a third position equation,
\begin{equation}
{\bf BASE2} \;\; {\bf PICK} \;=\; {\bf PLACE}, \label{third}
\end{equation}
which can be used to solve for {\bf PICK}.

Within a program, these three position equations
could be defined as follows:
\begin{verbatim}
        TRSF_PTR pick, place, base2, e1, e2;
        POS_PTR pickPos, placePos, refPos;

        placePos = makePosition ("placePos", T6, e1, EQ, place, TL, e1);
        pickPos = makePosition ("pickPos", T6, e2, EQ, pick, TL, e2);
        refPos = makePosition ("refPos", base2, pick, EQ, place, NULL);
\end{verbatim}
{\tt placePos} can be used as a target position
for the 
first manipulator's placing motion. Assume that the
value of the transform {\bf PLACE} is modified by a sensor
until the object is actually put down (ways of doing this will
be presented later). When the placement is finished, we can solve
for {\bf PICK} using
\begin{verbatim}
        solveTrans (pick, refPos, pick, NULL);
\end{verbatim}
and then {\tt pickPos} can be used as a target position for the second
manipulator's pickup motion.

This particular example is simple enough that if a programmer were
actually implementing it, he/she would probably dispense with
{\tt refPos} altogether and simply
call
\begin{verbatim}
        multLiTrsf (pick, base, place);
\end{verbatim}
directly. Whether or not the creation of explicit
position equations is advantageous depends heavily on the complexity
of the task. In particular, position equations
provide a compact way to describe
general kinematic loops to another module (such as when they are used
by the {\tt move()} primitive to describe a motion target
to the trajectory generator).

\newpage
\done
