fwdKinematics(3)

NAME
fwdKinematics, invKinematics, solveConf -- RCI routines to do kinematic computations.

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

ARGUMENTS

t6 --
    Coordinate frame describing the Cartesian coordinates of the last link of the robot.

confp --
    Place to return the manipulator's configuration code.

conf --
    Bit-code describing the configuration of the manipulator.

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

refj6 --
    (optional, set to NULL otherwise.) A set of reference joint values used in computing the inverse kinematics. In control applications, refj6 is usually the previous set of joint angle values. If this argument is omitted, the joint midpoint values, defined by the JLS structure, are used.

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

jls --
    Robot's JLS structure describing joint level parameters (used by the function invKinematics() to obtain the joint limits).

var --
    (optional, set to NULL otherwise.) Robot's VAR structure (see below), used to store byproduct joint angle sines and cosines.

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.

fwdKinematics() determines the last link coordinates t6 that correspond to the joint values j6. A code corresponding to the robot's configuration is also (optionally) returned through the pointer confp (which may otherwise be set to NULL). kyn and var are pointers to robot specific data structures (see below). var is an optional parameter, which, if not 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 h/<class>_kynvar.h. For PUMA robots, the following codes are defined:

PUMA_RIGHT_CONF --
    The robot is in the right-handed configuration.

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). DOWN here simply implies that joint 3 is on a particular side of its maximum extension.

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).

invKinematics() computes the inverse kinematics; it finds a set of joint values j6 that correspond to a given configuration code conf and last link coordinates 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 +/- 180 degrees of the reference values refj6. Typically, refj6 is set to the robot's most recent position, to minimize the amount the robot moves. If refj6 is set to 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 fwdKinematics(), var is an optional parameter, which, if not 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.

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

The Robot Data Structures

Each robot is associated with a unique JLS, KYN, and VAR structure. The JLS structure contains the joint level parameters (such as joint ranges and limits). The 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 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 JLS(3), KYN(3), and VAR(3).

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

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 180 degrees from the reference angle.

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

KYN_CANT_REACH --
    The desired position is completely out of reach, independant of joint limits.

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.

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 h/<class>_kynvar.h. Singularities which occur at the workspace boundary will usually trigger the KYN_CANT_REACH code instead.

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 +/- 180 degrees of the reference angles.

SEE ALSO
JLS(3), KYN(3), VAR(3), fwdJacob(3).