Automatically Producing Analytical Inverse Kinematics Solutions

Here is described a symbolic inverse kinematics solution finder for robotic manipulators. The program is called SymKin. It produces a C routine that calculates the joint values for a desired position of the end effector.

As a reference I used Richard P. Paul's book Robot Manipulators: Mathematics Programming and Control. It defines what Paul calls an A matrix that specifies the position of a robot link relative to the link previous to it towards the base of the manipulator.

For a 7 jointed manipulator the position of the last link relative to the base is -

T = A₁ * A₂ * A₃ * A₄ * A₅ * A₆ * A₇

A₁ is the position of the first link relative to the base, A₂ is the position of the second link relative to that of A₁ and so on. This is foward kinematics. Given the joint values (specified in the A matrices) the position of the last link (and therefore the end effector or gripper) can be determined by these matrix multiplications.

However in application the target position, T, is known and the joint values need to be determined. This is inverse kinematics.

When all the A matrices on the right side of the equation are multiplied together you have a single matrix on each side of the equation and each corresponding element of those two matrices can be equated. So there are 12 equations and 6 unknowns (6 joint values are the unknowns, one of the joints must be specified).

SymKin solves these equations and generates a C function with this signature -

int InvKin ( const double  Tgt[],
             const double  JCur[],
                   double  JNew[][2],
                   double  MaxDelta,
                   int     Odr[] )

Tgt[], the target position, are the elements of T.

JCur[] are the joints' current values.

JNew[] are new joint values determined by the function. In many situations there will be two possible values for some joints.

MaxDelta is the maximum allowable change in a joint value.

The values of the Odr[] array are flags for each joint. Some bits indicate in what order relative to other joints the joint was solved. Other bits are input flags indicating which of the possibly two joint values returned on the previous call was used.

So, the steps are as follows -

1. Design a manipulator. This implies determining the A matrices.
2. Call (or run standalone) SymKin. The inputs are the A matrices and for each A matrix a flag indicating if the joint variable is rotational or translational.
3. The results of SymKin is a file with the code of InvKin() in it. That file can be compiled and linked into another program.

An A matrix defines the position of a coordinate frame of a manipulator link relative to the coordinate frame of the previous link with rotations and translations in this order -

• Rotation about the current frame's X
• Translation along the previous frame's X
• Translation along the previous frame's Z
• Rotation about the previous frame's Z

In other words the A matrix of link n is -

A_n = Rz_n-1 * Tz_n-1 * Tx_n-1 * Rx_n

where, for example, Rz_n-1 means a rotation about the previous link's Z axis. One of these transforms represents the joint's variable. For rotational joints it is Rz_n-1, for translations it is Tz_n-1. The other three transforms are constants. So each A matrix has one variable.

Here are the A matrix parameters that were input to SymKin for the manipulator in the image at the top of this post -

    Link     Rz     Tz     Tx     Rx
    ----   ----   ----   ----   ----
       1      0     40      0   PI/2
       2      0      0      0   PI/2
       3      0      0    100      0
       4      0      0      0   PI/2
       5      0    120      0   PI/2
       6      0      0      0   PI/2
       7      0      0      0   PI/2 

Each joint in this manipulator rotates so Rz is the variable and its values are specified as 0.

Here is a video showing the manipulator moving to 10 random positions. For each move the destination is indicated by the red green blue coordinate frame. In between the manipulator's current position and the destination are calculated many intermediate target positions and InvKin() is called to determine 6 of the manipulator's 7 joint values for each of those target positions.