Programming Forward and Inverse Kinematics of PUMA 560?

Question

I am currently coding a Forward and Inverse Kinematics solver for a PUMA 560 robot. For the Inverse Kinematics part I am using the closed for solution given in this paper. But my issue is, my solution for IK for a given set of (x,y,z) does not return the same values returned by my FK values. The reason I am doing this is to verify my code accurately computes the FK and IK.

These are the DH parameters for my robot (These are Python code, since I was testing my algorithm on Spyder IDE before implementing on C++).

DH Parameters

Link lengths
a = [0, 650, 0, 0, 0, 0]

Link offsets
d = [0, 190, 0, 600, 0, 125]

Link twist angle
alpha = [-pi/2, 0, pi/2, -pi/2, pi/2, 0]

So basically I finding the T transformation matrix for each link from the the base frame {B} to wrist frame {W}. This is my code;

Function to compute Forward Kinematics

def forwardK(q):

#T06 is the location of Wrist frame, {W}, relative to Base frame, {B} 
T01 = genT(q[0],0,d[0],0)
T12 = genT(q[1],a[0],d[1],alpha[0])
T23 = genT(q[2],a[1],d[2],alpha[1])
T34 = genT(q[3],a[2],d[3],alpha[2])
T45 = genT(q[4],a[3],d[4],alpha[3])
T56 = genT(q[5],a[4],d[5],alpha[4])

#Tool frame {T}
#T67 = genT(0,0,d[5],0)

T03 = matmul(T01,T12,T23)
T36 = matmul(T34,T45,T56)
T06 = matmul(T03,T36)    
#T07 = matmul(T06,T67)

x = T[0][3]
y = T[1][3]
z = T[2][3]

print("X: ",x)
print("Y: ",y)
print("Z: ",z,"\n")
print("T: ",T,"\n")

return T06  

The function to compute T Matrix

def genT(theta, a, d, alpha):
T =  array([[cos(theta), (-sin(theta)), 0, a],
    [sin(theta)*cos(alpha), (cos(theta)*cos(alpha)), -sin(alpha), (-   d*sin(alpha))],
    [sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
    [0, 0, 0, 1]])

return T

from the T Matrix relating the {B} frame to the {W} frame position vector of the {w} [x y z] is extracted. R Matrix (orientation) of the {W} relative to the {B} is obtained by the following piece of code;

T = forwardK([30,-110,-30,0,0,0])
x = T[0][3]
y = T[1][3]
z = T[2][3]
R = T[0:3,0:3]

Where T is the transformation matrix relating {W} to {B}. Then this information is fed in to the invK(x,y,z,R,ARM,ELOBOW,WRIST) function to check if the algorithm returns the same set of angles fed to the forwardK(q1,q2,q3,q4,q5,q6) function.

In the invK(x,y,z,R,ARM,ELOBOW,WRIST) - ARM, ELBOW, WRIST are orientation specifiers to describe various possible configurations of the manipulator. Each of these parameters are either {+1,-1}. These values are then used in the closed form geometrical solution presented by the afore-mentioned paper.

I did not post the code for the invK(x,y,z,R,ARM,ELOBOW,WRIST) since it is a direct implementation of the closed form solution presented in the paper and also it significantly long hence making it highly unreadable.

What do you think I am doing wrong? I am quite sure the way I am computing the FK is correct but I could be wrong. The matrix multiplications of my Python code are correct since I double checked them with Matlab. Any advice is appointed.

Answer 1

You can't really make progress until you are sure the FK calculations are correct.

Think about the angles you are setting the joints to and where you expect the robot to be, and how you expect it to look. It is worth working through the DH transformations and drawing out all the frames in your own diagram.

Make sure you know whether you are using DH or Modified DH notation, as 50k4 mentions, and use one approach consistently. Make sure the link parameters match with the method.

If using DH you should have:

For Modified DH your reference is:



It looks like Lee and Zeigler are using DH so I would stick with that if you are wanting to use their solution.

To code up the FK and IK, these are the steps I normally use:

1. Split up your matrix multiplications and calculate the joint positions, joint by joint and test with known angles to check you have the correct positions.
2. Using the above calculations, plot your links using matplotlib or matlab. This will give you a visual reference.
3. Code up the solution for the FK and IK using the equations in Lee and Ziegler. You can test against this.
4. Generate a large range of angle inputs and output positions from your code
5. Write unit tests for your solvers using the output of 4

Once you have done this, work through the joints systematically trying different poses. Also once you have the FK solution correct, this will give you a test for your IK as you can then solve forwards and back and you should get to your original joint inputs.

If you are still stuck after doing this, then you will be in a better position to ask more specific questions here.

Answer 2

Take a look at the DH matrices on the Wikipedia page. I think, at a minimum, you have some sign errors. It also looks like maybe you use the same T matrix in both directions; from the base to the end effector and the other way. You can't do that. The way it's (almost) written, you have a T matrix that goes from the link coordinates backward; it doesn't go forward, but you're still using it in your forward kinematics calculations.

Try using one known set of parameters and invoke your genT function. Verify that the result gets you moved correctly, but I don't think it will.

:EDIT:

Your matrix is not the same. The matrix $^{n-1}T_n$ is:

$$ \left[ \begin{array}{cccc} \cos{\theta} & -\sin{\theta}\cos{\alpha} & \sin{\theta}\sin{\alpha} & r\cos{\theta} \\ \sin{\theta} & \cos{\theta}\cos{\alpha} & -\cos{\theta}\sin{\alpha} & r\sin{\theta} \\ 0 & \sin{\alpha} & \cos{\alpha} & d \\ 0 & 0 & 0 & 1 \end{array} \right] $$

which can be viewed as a conglomeration of the rotation matrix and translation vector:

$$ \left[ \begin{array}{c|c} R & T \\ \hline 0 & 1 \end{array} \right] $$ The inverse of that, $^nT_{n-1}$, is: $$ \left[ \begin{array}{cccc} \cos{\theta} & \sin{\theta} & 0 & -r \\ -\sin{\theta}\cos{\alpha} & \cos{\theta}\cos{\alpha} & \sin{\alpha} & -d\sin{\alpha} \\ \sin{\theta}\sin{\alpha} & -\cos{\theta}\sin{\alpha} & \cos{\alpha} & -d\cos{\alpha} \\ 0 & 0 & 0 & 1 \end{array} \right] $$

which is the same as:

$$ \left[ \begin{array}{c|c} R^{\mbox{T}} & -RT \\ \hline 0 & 1 \end{array} \right] $$

The inverse of a rotation matrix is the same as the transpose of the rotation matrix. The $\sin{\theta}$ term in the rotation matrix (at 2,1 or 1,2) is positive in both cases; yours is negative. That alone leads me to think your formulation is wrong. The signs on the other terms seem incorrect also.

As I mentioned before, try using a basic setup that you know the answer to, plug that into your T matrix function, and verify it gives you the correct output.

With software especially, any time you create a function you should try to perform a "Unit Test" to ensure that you are getting valid outputs from every sub-function you write.

My preference would be to do something graphically (hand-drawn), where maybe you have a link with a length of 1, and you rotate it 30, 45, 60, and 90 degrees. Verify that the resulting frames are all located in the correct places. You should consider also trying locations in all four quadrants - atan is the devil.