\input manualMacros.tex
\manBegin
\manPageInfo{RCI Reference Manual}{}{}
\manTitle{pumaKinematics}{3}
\manSection{NAME}
{\rightskip=0ptplus1fil
{\tt \manName{pumaFwdKinematics}},
{\tt \manName{pumaInvKinematics}},
{\tt \manName{pumaSolveConf}}

-- RCI routines to do PUMA kinematic computations.
}
\manSection{USAGE}
\cverbatim
pumaFwdKinematics (t6, confp, j6, kyn, var)
   TRSF *t6;
   unsigned long *confp;
   float *j6;
   PUMA_KYN *kyn;
   PUMA_VAR *var;

pumaInvKinematics (j6, t6, conf, refj6, kyn, jls, var)
   float *j6;
   TRSF *t6;
   unsigned long conf;
   float *refj6;
   PUMA_KYN *kyn;
   PUMA_VAR *var;
   JLS *jls;

pumaSolveConf(confp, j6, kyn)
   unsigned long *confp;
   float *j6;
   PUMA_KYN *kyn;
@endcverbatim
\manSection{ARGUMENTS}
\manItem{{\tt t6} --}
Coordinate
frame describing the Cartesian coordinates of link 6 of the robot.
\manItem{{\tt confp} --}
Place to return the manipulator's configuration code.
\manItem{{\tt conf} --}
Code describing the configuration of the manipulator.
\manItem{{\tt j6} --}
Robot joint angles, in radians.
\manItem{{\tt refj6} --}
A set of reference joint angles used in computing the inverse kinematics.
Usually {\tt refj6} is the previous joint angle values.
\manItem{{\tt kyn} --}
PUMA specific structure describing kinematic constants (see below). 
\manItem{{\tt jls} --}
Robot specific structure describing joint level parameters
(used by the function {\tt pumaInvKinematics()} to obtain the joint limits).
\manItem{{\tt var} --}
(optional) PUMA specific structure which is used to store (among
other things) the sines
and cosines of the joint angles (since they are computed by the kinematic
routines, we may as well store them for use by other routines).
\manSection{DESCRIPTION}
These routines perform the forward and inverse kinematic mappings for 
all types of PUMA robot. They may be called from any part of an RCI program.

{\tt pumaFwdKinematics()} determines the Cartesian coordinates {\tt t6} that
correspond to a set of joint angles {\tt j6}. A code corresponding
to the robot's configuration
is also (optionally) returned through the pointer {\tt confp} (which
may otherwise be set to {\tt NULL}). {\tt kyn} and {\tt var} are pointers
to robot specific data structures (see below). {\tt var} is an optional
parameter, which, if not {\tt NULL}, is used to store the sines and
cosines of all the angles which are computed as a by-product of the 
function's calculations.

The configuration code specifies the various redundancies
associated with a kinematic solution, describing these
with the setting of bits (defined in the file {\tt "puma_\-kynvar.h"}):

\manItem{{\tt PUMA_RIGHT_CONF} --}
The robot is in the right-handed configuration.
\manItem{{\tt PUMA_DOWN_CONF} --}
The robot's elbow is ``down'' (beware: this is relative; if
we hang the robot from the ceiling or swing joint 2 through
vertical, the intutive sense of ``up'' or ``down'' reverses).
{\tt DOWN} here simply implies that joint 3 is on a particular
side of its maximum extension.
\manItem{{\tt PUMA_FLIP_CONF} --}
The robot's link 5 is on a particular side of 0 (i.e., link 5 is
on a particular side of being straight out).

{\tt pumaInvKinematics()} computes the inverse kinematics;
it finds a set of joint angles {\tt j6} that correspond to a given
configuration code {\tt conf} and Cartesian link 6 position {\tt t6}.
Configurations are not the only redundancies that the
inverse kinematics must resolve; some joints may also be redundant
(i.e., their range of travel is greater than 360 degrees). This
is presently handled by the inversion routine by using a 
minimum travel algorthim: Joints are selected such that they are within
\manIfNroff +/- PI \manElse $\pm \pi$ \manFi of the reference angles
{\tt refj6}. Typically, the reference angles are the last angles
the robot was at; this minimizes the amount the robot moves. It is
not always what a user wants, however, and in fact the kinematics
routines should probably be extended to allow different ways of
hnadling joint redundancy (such as specifying a particular work
area for a joint, instead of using the closest one).
As with
{\tt pumaFwdKinematics()}, {\tt var} is an optional
parameter, which, if not {\tt NULL}, is used to store the sines and
cosines of all the angles which are computed as a by-product of the 
function's calculations. A diagnostic code is returned if the
kinematic solution is impossible; this is described below.

{\tt pumaSolveConf()} returns through {\tt confp} a configuration code
associated with the robot angles {\tt j6}.

\manSection{The Robot Data Structures}

Each robot is associated with a unique {\tt JLS},
{\tt KYN}, and {\tt VAR} structure, which contain, respectively,
the joint level parameters (such as joint ranges and limits), the
PUMA specific kinematic parameters (the A matrix terms), and
run time variables (such as the sines and cosines of the joint angles)
which we do not want to recompute from computation to computation.
RCI control tasks can obtain pointers to these structures from
the {\tt jls}, {\tt kyn}, and {\tt var} fields in the {\tt RCI_RBT}
structure (q.v.).
Non-RCI programs can obtains pointers to the structures be calling
{\tt get_jls()}, {\tt get_kyn()}, and {\tt get_var()} (q.v.),
with the name of the desired robot.

When obtaining pointers to {\tt PUMA_KYN} or {\tt PUMA_VAR} structures,
it is necessary to cast them appropriately since both the access routines
and the fields in {\tt RCI_RBT} declare them to be {\tt void*}.

\manSection{DIAGNOSTICS}
{\tt pumaInvKinematics()} returns a code if a solution is not
possible. Code values (defined in {\tt puma_kynvar.h}) include:

\manItem{bits 0-5 --}
The solution requires moving the indicated joint either out of
range, or more than \manIfNroff 180 degrees \manElse $180^\circ$ \manFi
from the reference angle. Bit 0 corresponds to joint 1, bit 1 to
joint 2, etc.

\manItem{{\tt PUMA_CANT_REACH} --}
The desired position is simply out of reach of the PUMA.

\manItem{{\tt PUMA_WRIST_DEG} --}
The desired position is at a wrist singularity, so that the solution
value for joint 4 (which is now redundant with joint 6) has been
chosen arbitrarily to be the value of the 4th reference angle.

\manSection{BUGS}
Using a minimum distance to a set of reference angles as a way of
resolving joint envelope ambiguities in inverse kinematics is
not always desirable; this should be fixed. It has an unpleasant side
effect of requiring that all solutions be within 
\manIfNroff +/- 180 degrees \manElse $\pm 180^\circ$ \manFi of the
reference angles.
\manSection{SEE ALSO}
\rightskip=0ptplus1fil
{\tt get_jls()}, {\tt get_kyn()}, {\tt get_var()}, {\tt RCI_RBT()}.
\manEnd
\end
