Matlab Code for Lynx Robot

Question

In this section, you will need to write a function lynx_fk which returns the positions of 10 points along the robot arm as defined later, given the joint angles of the robot.

For this question, please solve the forward kinematics of the robot using the frames as given below. The figure below shows the robot in its 'zero pose', i.e., making all joint angles 0 places the robot in this pose. Consider frame 0 to be the world frame. Also, are facing into the plane and away from you.

Input Format are the joint angles in radian, as shown in the figure

Output Format:

pos is a 10x3 matrix where each row contains the x,y,z coordinates represented as [x y z] in matrix form.

Each row is the x,y,z coordinates of a point in world frame (frame 0)

The first 5 rows must contain:

1. Position of frame 0 in world frame
2. Position of frame 1 in world frame
3. Position of frame 2 in world frame
4. Position of frame 3 in world frame
5. Position of frame 4 in world frame

The next 5 rows are used to describe the gripper (green dots shown in the figure above)

6. [0 0 -e] of frame 5 in world frame
7. [g/2 0 -e] of frame 5 in world frame
8. [-g/2 0 -e] of frame 5 in world frame
9. [g/2 0 0] of frame 5 in world frame
10. [-g/2 0 0] of frame 5 in world frame

My Code:

function [pos] = lynx_fk(theta1, theta2, theta3, theta4, theta5, g)

    pos = zeros(10, 3);

    A01 = compute_dh_matrix(0,     -pi/2, 3,     theta1);
    A12 = compute_dh_matrix(5.75,   0,    0,     theta2);
    A23 = compute_dh_matrix(7.375,  0,    0,     theta3);
    A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);
    A45 = compute_dh_matrix(0,      0,    4.125, theta5);

    L0 = [0;     0; 0;    1];
    L1 = [0;     0; 3;    1];
    L2 = [0;     0; 8.75; 1];
    L3 = [7.375; 0; 8.75; 1];
    L4 = [7.375; 0; 8.75; 1];
    L5 = [11.5;  0; 8.75; 1];

    pos(1,:) = [0,0,0];
    posi2 = (A01 * L1).';
    posi3 = ((A01*A12)* L2).';
    posi4 = ((A01*A12*A23) * L3).';
    posi5 = ((A01*A12*A23*A34) * L4).';

    pos(2,:) = posi2(:,1:3);
    pos(3,:) = posi3(:,1:3);
    pos(4,:) = posi4(:,1:3);
    pos(5,:) = posi5(:,1:3);

    posi5 = ((A01*A12*A23*A34*A45) * L5).';
    pos5 = posi5(:,1:3);

    pos(6,:)  = pos5 - [ 1.125, 0, 0];
    pos(7,:)  = pos5 + [-1.125, 0, g/2];
    pos(8,:)  = pos5 - [ 1.125, 0, g/2];
    pos(9,:)  = pos5 + [ 0,     0, g/2];
    pos(10,:) = pos5 - [ 0,     0, g/2];

    pos

end

The DH matrix is returned with the following function:

function A = compute_dh_matrix(r, alpha, d, theta)

    A = eye(4);

    dh_ct = cos(theta);
    dh_st = sin(theta);
    dh_ca = cos(alpha);
    dh_sa = sin(alpha);
    dh_r = r;
    dh_d = d;

    A(1) =  dh_ct;
    A(5) = -dh_st*dh_ca;
    A(9) =  dh_st*dh_sa;
    A(13) = dh_r*dh_ct;

    A(2) =   dh_st;
    A(6) =   dh_ct*dh_ca;
    A(10) = -dh_ct*dh_sa;
    A(14) =  dh_r*dh_st;

    A(3) =  0;
    A(7) =  dh_sa;
    A(11) = dh_ca;
    A(15) = dh_d;

    A(4) =  0;
    A(8) =  0;
    A(12) = 0;
    A(16) = 1;

end

The code for computation of DH matrix is correct, as it was checked previously. I suspect there might be a mistake in my DH values I found (which I found using right hand rule). Or there might be a problem in the rest of the code. Please help me debug or find my error.

Answer 1

I edited your post to clean up some of the formatting. I'll go over some glaring issues I see in the post, but if you're looking for really precise troubleshooting then you need to edit your question to post why you think the code you have isn't working correctly - i.e., you have an input, your code gives you an output, and you were expecting some other answer. Seeing what portions are correct and what aren't, etc. can help to narrow down the suspect code.

That said, I think a lot of bugs are now apparent after having cleaned up the code. Neatness and readability counts for a lot! I it's hard to look at your code, it's hard to find the bug.

For instance:

Problem 1.

A01 = compute_dh_matrix(0,     -pi/2, 3,     theta1);
A12 = compute_dh_matrix(5.75,   0,    0,     theta2);
A23 = compute_dh_matrix(7.375,  0,    0,     theta3);
A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);
A45 = compute_dh_matrix(0,      0,    4.125, theta5);

If you read down the columns now, you can quickly compare parameters with your drawing. Notice the A34 line?

A34 = compute_dh_matrix(0,     -pi/2, 3,     theta4);

But it looks like frames 3 and 4 are coincident! Why is there a distance of 3 between them?

Problem 2.

You seem to be misunderstanding how to use the frame transforms. You have the starting positions hard-coded in your lynx_fk function. This shouldn't be necessary. If you want to know the position of a point in Frame 1 with respect to the world (Frame 0), then you transform that point to world coordinates with the A01 transform:

pt_world = A01 * pt_frame1;

This chains - if you want to know how a point in Frame 2 is described in Frame 1, then you do the following:

pt_frame1 = A12 * pt_frame2;

And you can see then that, to express that point in world coordinates, you go:

pt_world = A01 * pt_frame1;
pt_world = A01 * (A12 * pt_frame2);

You can keep going on:

pt_world = A01 * A12 * ... * A_N_Nplus1 * pt_frameNplus1;

If you want to know where the frame origins are located in world coordinates, then you would do something like:

frame1_worldCoordinates = A01 * [0; 0; 0; 1];
frame2_worldCoordinates = A01 * A12 * [0; 0; 0; 1];

and so on. You can take those outputs and compare against your drawing and see where you're going wrong.

Problem 3.

You're overwriting pos5. I'm not going to say which way is correct, but your code has:

posi5 = ((A01*A12*A23*A34) * L4).';
<...>
pos(5,:) = posi5(:,1:3);

posi5 = ((A01*A12*A23*A34*A45) * L5).';
pos5 = posi5(:,1:3);

As stated above, neither will give you the location of the Frame 5 origin in world coordinates; that would need (list of transforms) * [0; 0; 0; 1].

Problem 4.

When you try to find the end effector locations, you don't have the positions in the correct columns. You have:

pos(6,:)  = pos5 - [ 1.125, 0, 0];
pos(7,:)  = pos5 + [-1.125, 0, g/2];
pos(8,:)  = pos5 - [ 1.125, 0, g/2];
pos(9,:)  = pos5 + [ 0,     0, g/2];
pos(10,:) = pos5 - [ 0,     0, g/2];

Which, when you distribute the negatives, is equivalent to:

pos(6,:)  = pos5 + [-1.125, 0,  0];
pos(7,:)  = pos5 + [-1.125, 0,  g/2];
pos(8,:)  = pos5 + [-1.125, 0, -g/2];
pos(9,:)  = pos5 + [ 0,     0,  g/2];
pos(10,:) = pos5 + [ 0,     0, -g/2];

But you're supposed to have:

[ 0   0 -e] of frame 5 in world frame
[ g/2 0 -e] of frame 5 in world frame
[-g/2 0 -e] of frame 5 in world frame
[ g/2 0  0] of frame 5 in world frame
[-g/2 0  0] of frame 5 in world frame

This is probably because you're compensating for:

Problem 5.

You're not converting the end effector locations to world coordinates. Just like Problem 2 says, if you want to convert a point from a particular frame to world coordinates, step through the transforms from the local expression of that point to each subsequent parent frame until you've arrived at the world coordinate frame.

So, for example, [0 0 -e] of frame 5 in world frame. To find the origin of Frame 5, you take a Frame 5 point that is located at the origin ([0; 0; 0]) and pass it through the sequence of transforms to get back to world coordinates. You do the same thing here, but with [0; 0; -e], etc., to get the expression for those points in world coordinates.

If you can fix all of these problems and you're still having issues, please edit your question to provide your updated code, a sample input, the output of your function for the sample input, and the output you expected to get for your sample input.

The best hint I can give you is that, if you have your DH values correct and everything is working correctly, then passing all zeros for the thetaN values should give you the general arrangement drawing you've provided in your original post. You shouldn't have to hard-code anything except the DH parameters in the lynx_fk code.