\input manualMacros.tex
\manBegin
\manPageInfo{RCI Reference Manual}{}{}
\manTitle{fwdKinematics}{3}
\manSection{NAME}
{\rightskip=0ptplus1fil
{\tt \manName{fwdKinematics}},
{\tt \manName{invKinematics}},
{\tt \manName{solveConf}}
-- RCI routines to do kinematic computations.

}
\manSection{USAGE}
\cverbatim
#include <robot_kynvar.h>

fwdKinematics (t6, confp, j6, kyn, var)
   TRSF *t6;
   unsigned long *confp;
   float *j6;
   KYN *kyn;
   void *var;

invKinematics (j6, t6, conf, refj6, kyn, jls, var)
   float *j6;
   TRSF *t6;
   unsigned long conf;
   float *refj6;
   KYN *kyn;
   JLS *jls;
   void *var;

solveConf(confp, j6, kyn)
   unsigned long *confp;
   float *j6;
   KYN *kyn;
@endcverbatim
\manSection{ARGUMENTS}
\manItem{{\tt t6} --}
Coordinate
frame describing the Cartesian coordinates of the last link of the robot.
\manItem{{\tt confp} --}
Place to return the manipulator's configuration code.
\manItem{{\tt conf} --}
Bit-code describing the configuration of the manipulator.
\manItem{{\tt j6} --}
Robot joint values (radians or millimeters).
\manItem{{\tt refj6} --}
(optional, set to {\tt NULL} otherwise.) 
A set of reference joint values used in computing the inverse kinematics.
In control applications, {\tt refj6} is usually the previous set of
joint angle values. If this argument is omitted, the joint midpoint
values, defined by the {\tt JLS} structure, are used.
\manItem{{\tt kyn} --}
Robot's {\tt KYN} structure (see below).
\manItem{{\tt jls} --}
Robot's {\tt JLS} structure describing joint level parameters
(used by the function {\tt invKinematics()} to obtain the joint limits).
\manItem{{\tt var} --}
(optional, set to {\tt NULL} otherwise.) 
Robot's {\tt VAR} structure (see below), used to store
byproduct joint angle sines and cosines.
\manSection{DESCRIPTION}
These routines perform the forward and inverse kinematic mappings for 
the 6 DOF robots supported by RCI. They may be called from any part of an RCI
program. 

{\tt fwdKinematics()} determines the last link coordinates {\tt t6} that
correspond to the joint values {\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 the robot joint angles, which are computed as a byproduct of the 
function's calculations.

The configuration code specifies the various redundancies
associated with a kinematic solution. The codes for a particular
class of robot are defined in the file {\filefont h/<class>_kynvar.h}.
For PUMA robots, the following codes are defined:

\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 invKinematics()} computes the inverse kinematics;
it finds a set of joint values {\tt j6} that correspond to a given
configuration code {\tt conf} and last link coordinates {\tt t6}.
Configurations are not the only redundancies that the
inverse kinematics must resolve; some revolute joints may also be redundant
(i.e., their range of travel is greater than 360 degrees). This
is presently handled by using a 
minimum travel algorthim: joints are selected such that they are within
\manIfNroff +/- 180 degrees \manElse $\pm 180^\circ$ \manFi of the reference
values {\tt refj6}. Typically, {\tt refj6} is set to the robot's
most recent position, to minimize the amount the robot moves. If
{\tt refj6} is set to {\tt NULL}, then the joint mid-range values are
used instead. Other techniques for handling joint angle redundancy 
may be introduced in the future.
As with
{\tt fwdKinematics()}, {\tt var} is an optional
parameter, which, if not {\tt NULL}, is used to store joint angle sines and
cosines computed as a byproducts. A diagnostic code is returned if the
kinematic solution is impossible; this is described below.

{\tt solveConf()} returns through {\tt confp} a configuration code
associated with the joint values {\tt j6}.

\manSection{The Robot Data Structures}

Each robot is associated with a unique {\tt JLS},
{\tt KYN}, and {\tt VAR} structure. The {\tt JLS} structure
contains the joint level parameters (such as joint ranges and limits).
The {\tt KYN} structure contains 
robot specific kinematic and dynamic parameters (such as A matrix terms) and
pointers to internal functions to do kinematic and dynamic calculations.
The {\tt VAR} structure contains
run time variables (such as the sines and cosines of the joint angles)
which we do not want to recompute from computation to computation.
For complete details on how to obtain and handle these data structures,
see the manual pages {\tt JLS(3)}, {\tt KYN(3)}, and {\tt VAR(3)}.

\manSection{DIAGNOSTICS}
{\tt invKinematics()} returns a non-zero bit-code if a solution is not
possible or is possibly badly conditioned. 
Bit codes are defined in {\filefont h/robot_kyn.h}. The following codes
indicate no possible solution:

\manItem{{\tt KYN_JOINT_LIMIT} --}
A set of bits reserved to indicate joint travel limits, with the lowest bit
corresponding to joint 1, the next bit corresponding to joint 2, etc.
If any of these bits are set, it means that 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.

The remaining codes indicate that a solution was found, but that
it may be badly conditioned:

\manItem{{\tt KYN_CANT_REACH} --}
The desired position is completely out of reach, independant of
joint limits. 

\manItem{{\tt KYN_WRIST_SING} --}
A special bit to indicate a wrist singularity. Typically, this
will imply that two of the robot's wrist joints have aligned and 
that the value for one of them is arbitrary. Usually, in this case, one of the
joint values is taken from the reference angles.

\manItem{{\tt KYN_SINGULAR} --}
A set of bits to indicate singularities other than the wrist singularity.
Specific codes are defined for each class of robot in the file
{\filefont h/<class>_kynvar.h}. Singularities which occur at
the workspace boundary will usually trigger the {\tt KYN_CANT_REACH}
code instead.

\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 JLS(3)}, {\tt KYN(3)}, {\tt VAR(3)}, {\tt fwdJacob(3)}.

\manEnd
\end
