Forward Kinematics for Baxter

Question

I've put together this Forward Kinematics function for Baxter arm robot based on its hardware specs and the following joints axis: The joint positions for the following forward kinematics are not matching the corresponding Cartesian coordinates, what am I doing wrong here?

def FK_function_2(joints):
    def yaw(theta): #(rotation around z)
        y = np.array([[np.cos(theta), -np.sin(theta), 0],
                      [np.sin(theta), np.cos(theta), 0],
                      [0, 0, 1] ])
        return y

    R01 = yaw(joints[0]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R12 = yaw(joints[1]).dot(np.array([[0,      0,   -1],
                                       [-1,     0,   0],
                                       [0,      1,   0]]))
    R23 = yaw(joints[2]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R34 = yaw(joints[3]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R45 = yaw(joints[4]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R56 = yaw(joints[5]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R67 = yaw(joints[6]).dot(np.array([[1,      0,   0],
                                       [0,      1,   0],
                                       [0,      0,   1]]))

    d = np.array([0.27035, 0, 0.36435, 0, 0.37429, 0, 0.229525])
    a = np.array([0.069, 0, 0.069, 0, 0.010, 0, 0])

    l1 = np.array([a[0]*np.cos(joints[0]), a[0]*np.sin(joints[0]), d[0]]);
    l2 = np.array([a[1]*np.cos(joints[1]), a[1]*np.sin(joints[1]), d[1]]); 
    l3 = np.array([a[2]*np.cos(joints[2]), a[2]*np.sin(joints[2]), d[2]]); 
    l4 = np.array([a[3]*np.cos(joints[3]), a[3]*np.sin(joints[3]), d[3]]); 
    l5 = np.array([a[4]*np.cos(joints[4]), a[4]*np.sin(joints[4]), d[4]]);
    l6 = np.array([a[5]*np.cos(joints[5]), a[5]*np.sin(joints[5]), d[5]]);
    l7 = np.array([a[6]*np.cos(joints[6]), a[6]*np.sin(joints[6]), d[6]]);

    unit = np.array([0, 0, 0, 1])
    H0 = np.concatenate((np.concatenate((R01, l1.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H1 = np.concatenate((np.concatenate((R12, l2.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H2 = np.concatenate((np.concatenate((R23, l3.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H3 = np.concatenate((np.concatenate((R34, l4.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H4 = np.concatenate((np.concatenate((R45, l5.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H5 = np.concatenate((np.concatenate((R56, l6.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H6 = np.concatenate((np.concatenate((R67, l7.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)


    T = H0.dot(H1).dot(H2).dot(H3).dot(H4).dot(H5).dot(H6)

    return T[0:3, 3]

Answer 1

Ok, so I have been looking at this and checked your code. The code is good and works with your defined kinematic chain with transformations from the base to the end of the robotic arm.

(H0 * H1 * H2 * H3 * H4 * H5 * H6) is the correct kinematic chain where each represents a transformation from one joint to the next in the chain starting at the base of the arm.

The problem is your transformations are wrong. Your representation of H0 through H6 is not right and it is the numbers in these matrices that cause your transformations to not match the real transformations that take place. You need to from up the correct transformations from the base all the way to the end of the arm. Other than that, your approach is correct.

It looks like you are using normal DH parameters for your transformation matrices. Your values for a and d (and alpha which isn't shown in your code) are off and causing the transformations to be expressed incorrectly. The DH parameters are seen in https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters.

I found an exact guide for Baxter's forward kinematics to help after going through the modified DH table to set up the transformations. I would look at the modified DH parameters at the end of the wiki article above since the guide uses that.

Baxter Forward Kinematic Guide: https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

In this paper, the author, Robert Williams, sets up the DH parameters for the Baxter robotic arm and gets values different than what you have (I know you are using the normal DH parameters, but I would look at using the modified ones). His table is:

With lengths of:

And using the modified DH matrix:

Now you can calculate matrices H0 through H6 and if you want you can also add the end effector geometry if you have that for an additional H7. Once you multiple them all together you should get the proper forward kinematic transformation (see the paper for additional resource). Both the left and right arms have the same kinematics.

When you multiply it all together then you get the expressions for the coordinates of x7, y7, and z7 from the base of the arm that are functions of the rotations of the joints and the geometry of the robot arm. See the paper on page 17 for the expressions for x7,y7, and z7. Also see page 14 for the individual transformations.

Also don't forget to express the angles in radians since your code uses regular trig functions.

One last update: I just remembered that it is easier for me to think of the intermediate translation and rotational steps one-by-one (instead of jumping straight to the DH matrix). The two approaches will be equivalent, but I like to think of each individual step that it takes to get from one rotation frame to the next.

For this you can use these building blocks.

Pure Translation:

[1   0   0   u;
0    1   0   v;
0    0   1   w;
0    0   0    1]

Where u is the distance from the previous frame to the new frame measured from the previous x frame axis.

Where v is the distance from the previous frame to the new frame measured from the previous y frame axis.

Where w is the distance from the previous frame to the new frame measured from the previous z frame axis.

Rotation about the z-axis by arbitrary theta: This represent the robot joint rotation to an arbitrary theta.

[cos(theta)    -sin(theta)        0 0;
sin(theta)     cos(theta)         0 0;
0                   0             1 0;
0                   0             0 1]

Combination of rotations around intermediate frames to get to final frame position: (these angles will usually be in increments of pi/2 or pi to be able to get to the final orientation) Can use a rotation about the intermediate x axis, y axis, or z axis shown below.

(Rotation about x axis by alpha)

R_x(alpha) =         [1             0           0              0;
                      0         cos(alpha)  -sin(alpha)        0;
                      0         sin(alpha)  cos(alpha)         0;
                      0            0            0              1];

(Rotation about y axis by beta)

R_y(beta) =   [  cos(beta)     0      sin(beta)    0;
                     0         1          0        0;
                 -sin(beta)    0      cos(beta)    0;
                     0         0          0        1];

(Rotation about z axis by gamma):

[cos(gamma)  -sin(gamma)     0      0;
sin(gamma)    cos(gamma)     0      0;
       0          0          1      0;
       0          0          0      1]

So with these building blocks you can build the sequence of steps to go from one frame to another (essentially any H matrix can be decomposed into these steps). The chain would be something like:

[H](transformation from previous frame to the next frame) = [Pure Translation from previous joint to new joint expressed in the previous joint's frame] * [Rotation about previous frame's z-axis by theta (for the joint) (since the joint has many positions, theta is left as symbolic)] * [All other intermediate rotations to arrive at the new joint frame orientation expressed as rotations about intermediate axis frames]

That is essentially what the DH parameters helps you do, but I like to think of the individual steps to get from one frame to the next instead of jumping there with the DH parameters.

With the corrected H0 through H6 transformations, your approach is correct. Just change the definitions of H0 through H6 in your code.