\input manualMacros.tex
\manBegin
\manPageInfo{RCI Reference Manual}{}{}
\manTitle{pumaJacob}{3}
\manSection{NAME}
{\rightskip=0ptplus1fil
{\tt \manName{pumaFwdJacob}},
{\tt \manName{pumaInvJacob}},
{\tt \manName{pumaFwdJacobT}},
{\tt \manName{pumaInvJacobT}}

-- RCI routines to do PUMA Jacobian computations.
}
\manSection{USAGE}
\cverbatim
#include <puma_kynvar.h>

pumaFwdJacob(d, q, kyn, var)
   DIFF_PTR d;
   float *q;
   PUMA_KYN *kyn;
   PUMA_VAR *var;

pumaFwdJacobT(j, f, kyn, var)
   float *j;
   FORCE *f;
   PUMA_KYN *kyn;
   PUMA_VAR *var;

pumaInvJacob(q, d, kyn, var, epsilon)
   float *q;
   DIFF_PTR d;
   PUMA_KYN *kyn;
   PUMA_VAR *var;
   float epsilon;

pumaInvJacobT(f, t, kyn, var, epsilon)
   FORCE *f;
   float *t;
   PUMA_KYN *kyn;
   PUMA_VAR *var;
   float epsilon;
@endcverbatim
\manSection{ARGUMENTS}
\manItem{{\tt d} --}
Differential coordiante changes in Cartesian space.
\manItem{{\tt q} --}
Differential coordinate changes in joint space.
\manItem{{\tt j} --}
(Nominally) forces in joint space.
\manItem{{\tt f} --}
(Nominally) forces in Cartesian space.
\manItem{{\tt kyn} --}
PUMA specific structure describing kinematic constants (see below). 
\manItem{{\tt var} --}
PUMA specific structure containing run time variables which depend
on joint position, such as the sines and cosines of all the
angles, and various elements of the Jacobian (see below).
\manItem{{\tt espilon} --}
Specifies, in radians, how close inverse computations are allowed to
get to a singularity (see below).
\manSection{DESCRIPTION}
These routines perform the following Jacobian computations for the
PUMA robot. They may be called from any part of an RCI program.

{\tt pumaInvJacob()} performs the inverse computation:
\manIfTeX
$$
{\bf q} \;=\; {\bf J}^{-1} \; {\bf d}
$$
\manElse
\cverbatim
                                     -1
                                q = J   d
@endcverbatim
\manFi

{\tt pumaFwdJacob()} maps differential changes in joint coordinates into
the corresponding differential changes in Cartesian coordinates:
\manIfTeX
$$
{\bf d} \;=\; {\bf J} \; {\bf q}
$$
\manElse
\cverbatim
                                d = J q
@endcverbatim
\manFi

{\tt pumaFwdJacobT()} is typically used to map Cartesian space forces
onto their equivalent joint space forces:
\manIfTeX
$$
{\bf j} \;=\; {\bf J}^T \; {\bf f}
$$
\manElse
\cverbatim
                                     T
                                j = J  f
@endcverbatim
\manFi

{\tt pumaInvJacobT()} performs the inverse force computation:
\manIfTeX
$$
{\bf f} \;=\; {\bf J}^{-1T} \; {\bf j}
$$
\manElse
\cverbatim
                                     -1T
                                f = J    j
@endcverbatim
\manFi

The inverse computations may be impeded by singularities. 
If a singularity
is detected, then the function returns a bit code describing the 
singularites found. These bits, defined in {\tt puma_kynvar.h}, are:

\manItem{{\tt PUMA_ALIGN_DEG} --}
The center of the wrist is in the plane defined by the intersection of the 
axes of joints 1 and 2. Further motion towards axis 1 is impossible,
a degree of freedom is lost, and hence the singularity. This is the most
obscure of the PUMA singularities.

\manItem{{\tt PUMA_ELBOW_DEG} --} 
This is the most obvious of the PUMA singularities, occuring when the
elbow is ``straight out''. 

\manItem{{\tt PUMA_WRIST_DEG} --}
This occurs when joint 5 is at 0 (i.e., straight out).

Note that the classic ``ready'' position on the PUMA 560 contains
all three singularities.

Singularities are detected within bounds specified by the
{\TT epsilon} argument.
{\tt epsilon}
represents the closest distance, in radians, that the joint associated
with a particular singularity is allowed to get to a singular position
(joint 5 for the {\it wrist} singularity; joint 3 for the {\it elbow}
singularity; joint 2 for the {\it alignment} singularity, with angle 3
assumed fixed).
If any of the angles are within {\tt epsilon} of a singularity,
then the singularity code is set and the computation is adjusted
(see the next paragraph). The value of {\tt epsilon} must be positive,
and its value is set to a minimum of 2.0e-6 (at the time of this writing).

Adjustment of the Jacobian computation in the presence of a singularity
works along the following lines: Since a singularity implies some (but not
all) degrees of freedom are lost, we still wish to compute information
based on the degrees of freedom that remain. The right way to do this
is to take a pseudo-inverse. That, however, is a little beyond our
real-time limitations. Instead, we use the following technique which
sort of works, though is probably rather rude from a mathematical
point of view:

Any given singularity manifests itself inside the routine as a 
denominator tending towards 0. A computation
is considered to lie in a singular region if the absolute
value of the denominator (called $h$ here) lies within a threshold
$t$. Within the singular region the result of the computation is not
considered meaningful, although we would like to allow the result
to change signs (pass through 0) gracefully so as to not render
totally meaningless other computations which may include its result.
Consequently, within a singular region, we {\it multiply} by the denominator
in the following way ($n$ is some numerator):
\manIfNroff
\cverbatim

                           n / h ,   |h| > t

                                2
                         n h / t  ,  |h| <= t
@endcverbatim
\manElse
$$
n / h, \qquad |h| > t
$$
$$
n h / t^2, \qquad |h| <= t
$$
\manFi

\manSection{The KYN and VAR structures}
The {\tt kyn} structure contains robot kinematic constants which vary from one
robot to another.
RCI control tasks
can obtain a pointer to this structure from the {\TT kyn} field
in the {\tt RCI_RBT} structure (q.v.). Regular UNIX programs can
obtain a pointer to this structure for a particular robot with the
call {\tt get_kyn(<robotName>)} (q.v.).

The {\tt var} structure exists so that time critical terms which
depend on joint positions do not have to be recomputed everytime
a Jacobian computation is made.
RCI control tasks
can obtain a pointer to this structure from the {\TT var} field
in the {\tt RCI_RBT} structure (q.v.). Regular UNIX programs can
obtain a pointer to this structure for a particular robot with the
call {\tt get_var(<robotName>)} (q.v.). All the terms in the structure can
be updated for a particular value set of angles using the function
{\tt pumaUpdateVar()} (q.v.). Alternatively, if the sines and cosines
of the structure have already been determined elsewhere in the control
process (as can be done optionally 
by the kinematics routines), then the rest of the structure can be
updated with the function {\tt pumaUpdateJacob()}.
\manSection{DIAGNOSTICS}
The inverse compuations {\tt pumaInvJacob()} and {\tt pumaInvJacobT()}
return codes indicating any detected singularites.
\manSection{BUGS}
The singularity trick described above is a bit of a hack.
\manSection{SEE ALSO}
\raggedright
{\tt get_kyn()}, {\tt get_var()}, {\tt RCI_RBT()}, {\tt pumaUpdateVar()}.
\manEnd
\end
