\input manualMacros.tex
\manBegin
\manPageInfo{RCI Reference Manual}{}{}
\manTitle{fwdJacob}{3}
\manSection{NAME}
{\rightskip=0ptplus1fil
{\tt \manName{fwdJacob}},
{\tt \manName{invJacob}},
{\tt \manName{fwdJacobT}},
{\tt \manName{invJacobT}}
-- RCI routines to do Jacobian computations.

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

fwdJacob(d, q, j6, kyn)
   DIFF *d;
   float *q;
   float *j6;
   KYN *kyn;

fwdJacobT(j, f, j6, kyn)
   float *j;
   FORCE *f;
   float *j6;
   KYN *kyn;

invJacob(q, d, j6, kyn, epsilon)
   float *q;
   DIFF *d;
   float *j6;
   KYN *kyn;
   float epsilon;

invJacobT(f, t, j6, kyn, epsilon)
   FORCE *f;
   float *t;
   float *j6;
   KYN *kyn;
   float epsilon;
@endcverbatim
\manSection{ARGUMENTS}
\manItem{{\tt d} --}
Differential coordinate 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 j6} --}
Robot joint values (radians or millimeters).
\manItem{{\tt kyn} --}
Robot's {\tt KYN} structure (described below).
\manItem{{\tt espilon} --}
Specifies, in radians, how close inverse computations are allowed to
get to a singularity before the result is ``fudged'' (see below).
\manSection{DESCRIPTION}
These routines perform Jacobian computations for the
6 DOF robots supported by RCI. They may be called from any part of an RCI
program. 

The {\tt kyn} structure contains robot kinematic and
dynamic constants. 
For full details on the {\tt KYN} structure and how to obtain it,
see the manual page {\tt KYN(3)}.

{\tt fwdJacob()} 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 invJacob()} performs the inverse computation:
\manIfTeX
$$
{\bf q} \;=\; {\bf J}^{-1} \; {\bf d}
$$
\manElse
\cverbatim
                                     -1
                                q = J   d
@endcverbatim
\manFi

{\tt fwdJacobT()} 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 invJacobT()} 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. What the code actually means depends on the
robot; codes for a particular robot class are defined in the
file {\filefont h/<class>_kynvar.h}.

For the PUMA robot, the singularity codes, 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, where
the arm is pointing straight up, 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.
This notion can be quite approximate since not all singularities are related 
to the position of a single joint.
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 that 
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 use a matrix singular value decomposition to identify the degenerate
degrees of freedom. That, however, is beyond the computational
ability of these routines.
Instead, we use the following ``rough'' technique:

Any given singularity manifests itself inside the routine as 
some denominator $d$ tending towards 0. A computation
is considered to lie in a singular region if the absolute
value of $d$ 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 / d ,   |d| > t

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

\manSection{DIAGNOSTICS}
The inverse compuations {\tt invJacob()} and {\tt invJacobT()}
return codes indicating any detected singularites.
\manSection{BUGS}
The singularity trick described above is a bit of a hack.
\manSection{SEE ALSO}
\rightskip=0ptplus1fil
{\tt KYN(3)}, {\tt fwdKinematics(3)}.

\manEnd
\end
