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:
- Position of frame 0 in world frame
- Position of frame 1 in world frame
- Position of frame 2 in world frame
- Position of frame 3 in world frame
- Position of frame 4 in world frame
The next 5 rows are used to describe the gripper (green dots shown in the figure above)
- [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
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.