fwdJacob(3)

NAME
fwdJacob, invJacob, fwdJacobT, invJacobT -- RCI routines to do Jacobian computations.

USAGE
#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;

ARGUMENTS

d --
    Differential coordinate changes in Cartesian space.

q --
    Differential coordinate changes in joint space.

j --
    (Nominally) forces in joint space.

f --
    (Nominally) forces in Cartesian space.

j6 --
    Robot joint values (radians or millimeters).

kyn --
    Robot's KYN structure (described below).

espilon --
    Specifies, in radians, how close inverse computations are allowed to get to a singularity before the result is "fudged" (see below).

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 kyn structure contains robot kinematic and dynamic constants. For full details on the KYN structure and how to obtain it, see the manual page KYN(3).

fwdJacob() maps differential changes in joint coordinates into the corresponding differential changes in Cartesian coordinates:
                                d = J q

invJacob() performs the inverse computation:
                                     -1
                                q = J   d

fwdJacobT() is typically used to map Cartesian space forces onto their equivalent joint space forces:
                                     T
                                j = J  f

invJacobT() performs the inverse force computation:
                                     -1T
                                f = J    j

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 h/<class>_kynvar.h.

For the PUMA robot, the singularity codes, defined in puma_kynvar.h, are:

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.

PUMA_ELBOW_DEG --

    This is the most obvious of the PUMA singularities, occuring when the elbow is "straight out".

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 "epsilon" argument. 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 epsilon of a singularity, then the singularity code is set and the computation is adjusted (see the next paragraph). The value of 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 multiply by the denominator in the following way (n is some numerator):
                           n / d ,   |d| > t

                                2
                         n d / t  ,  |d|

DIAGNOSTICS
The inverse compuations invJacob() and invJacobT() return codes indicating any detected singularites.

BUGS
The singularity trick described above is a bit of a hack.

SEE ALSO
KYN(3), fwdKinematics(3).