# Forward Kinematics for Two Axis Gimbal Mount

I'm trying to work out the orientation of a block at the end of two axis gimbal. I know the position of the two rotational joints, but would like to calculate the final orientation of the block given the position of the two axis gimbal. Hence I thought calculating the forward kinematics would be an appropriate solution.

The gimbal system I'm trying to calculate the forward kinematics for is shown in the image below.

I've attempted to model the gimbal diagrammatically below.

The Denavit-Hartenberg parameter table I calculate as:

θ          α          r          d
90         -90        0          0
180 + θ1   90         0          0
180 + θ2   180        0          0


I've implemented this in Matlab as:

clear all;
clc;

inner = 90;
outer = 180;

theta = 90;
alpha = 90;

h01 = DH(theta, alpha);

theta = 180 + outer;
alpha = -90;

h12 = DH(theta, alpha);

theta = 180 + inner;
alpha = 180;

h13 = DH(theta, alpha);

effector = h01*h12*h13;

disp(effector);

function h = DH(theta, alpha)

h= [cosd(theta), -sind(theta)*cosd(alpha), sind(theta)*sind(alpha);
sind(theta), cosd(theta)*cosd(alpha), -cosd(theta)*sind(alpha);
0,           sind(alpha),              cosd(alpha)];

end


However, I'm getting the wrong answer as if I apply an inner axis rotation of 90 degrees and an outer axis rotation of 180 degrees I should get the identity matrix i.e. my block frame should align with the frame I've set for the base. Can anyone see where I'm going wrong and give some pointers?

• Why not just change the last line in the DH matrix? – Chuck Oct 21 '20 at 12:31
• Hi @Chuck thanks for the reply. I'm not sure which line you're referring to? Is there a mistake with my derivation in the last line? – Joe Oct 21 '20 at 13:20

To expand on my comment, the last line of your DH table gives the transform between END_EFFECTOR-1 and END_EFFECTOR frames, right? I'm saying instead of <θ2, 0, 0, 0> to have <180+θ2, 180, 0, 0>.

There's no movement between your end effector and the object you're attaching to the end effector, so there's no point in keeping the end effector frame. Dragging it along through all your calculations isn't going to add anything for you.

The other easy alternative is to add an extra line that defines the relationship between the end effector and the object.

:EDIT:

I modified your script a bit to make some visualizations of the frames. I used subplot in Matlab to try to orient the plots to roughly what you've drawn above, and then I could see immediately where you were having trouble. I changed your theta/alpha values to get the frames to match up, and I renamed h13 to h23 because it's the transform between frames 2 and 3, not 1 and 3.

clear all;
close all;
clc;

inner = 0;
outer = 0;

theta = 90;
alpha = 90;

h01 = DH(theta, alpha);

theta = 180 + outer;
alpha = 90;

h12 = DH(theta, alpha);

theta = 180 + inner;
alpha = 180;

h23 = DH(theta, alpha);

effector = h01*h12*h23;

disp(effector);

x = [1, 0, 0].';
y = [0, 1, 0].';
z = [0, 0, 1].';

viewAZ = 30;
viewEL = 30;
viewSettings = [viewAZ, viewEL];
xMin = -1;
xMax = 1;
yMin = -1;
yMax = 1;
zMin = -1;
zMax = 1;
axisLimits = [xMin, xMax, yMin, yMax, zMin, zMax];

figure(1)
subplot(3,2,5)
plot3([0, x(1)],[0, x(2)],[0, x(3)],'Color',[1 0 0]);
hold on;
plot3([0, y(1)],[0, y(2)],[0, y(3)],'Color',[0 1 0]);
plot3([0, z(1)],[0, z(2)],[0, z(3)],'Color',[0 0 1]);
axis equal;
axis(axisLimits)
view(viewSettings);
title('F0 - Base')

x1 = h01*x;
y1 = h01*y;
z1 = h01*z;

subplot(3,2,3)
plot3([0, x1(1)],[0, x1(2)],[0, x1(3)],'Color',[1 0 0]);
hold on;
plot3([0, y1(1)],[0, y1(2)],[0, y1(3)],'Color',[0 1 0]);
plot3([0, z1(1)],[0, z1(2)],[0, z1(3)],'Color',[0 0 1]);
axis equal;
axis(axisLimits)
view(viewSettings);
title('F1')

x2 = (h01*h12)*x;
y2 = (h01*h12)*y;
z2 = (h01*h12)*z;

subplot(3,2,4)
plot3([0, x2(1)],[0, x2(2)],[0, x2(3)],'Color',[1 0 0]);
hold on;
plot3([0, y2(1)],[0, y2(2)],[0, y2(3)],'Color',[0 1 0]);
plot3([0, z2(1)],[0, z2(2)],[0, z2(3)],'Color',[0 0 1]);
axis equal;
axis(axisLimits)
view(viewSettings);
title('F2')

x3 = (h01*h12*h23)*x;
y3 = (h01*h12*h23)*y;
z3 = (h01*h12*h23)*z;

subplot(3,2,2)
plot3([0, x3(1)],[0, x3(2)],[0, x3(3)],'Color',[1 0 0]);
hold on;
plot3([0, y3(1)],[0, y3(2)],[0, y3(3)],'Color',[0 1 0]);
plot3([0, z3(1)],[0, z3(2)],[0, z3(3)],'Color',[0 0 1]);
axis equal;
axis(axisLimits)
view(viewSettings);
title('F3 - End Effector')

function h = DH(theta, alpha)

h= [cosd(theta), -sind(theta)*cosd(alpha), sind(theta)*sind(alpha);
sind(theta), cosd(theta)*cosd(alpha), -cosd(theta)*sind(alpha);
0,           sind(alpha),              cosd(alpha)];

end

• Hi @Chuck, thanks for your help! I've updated the last line as you suggested which is much neater and I also noticed I had inner and outer angles in the wrong place in my code (should be swapped). I only didn't have the object frame as the end effector frame as I thought the final frame had to be the same as the previous frame (from the tutorial I was following)? – Joe Oct 21 '20 at 20:33
• I now seem to be getting slightly more sensible answers i.e. for an outer angle of 180 degrees and inner angle of 90 degrees I get the matrix [-1, 0,0; 0, -1, 0; 0, 0, 1], when I think I should get [1, 0, 0; 0, 1, 0; 0, 0 1];. Similarly for an outer angle of outer angle of 180 degrees and inner angle of 180 degrees I get the matrix [0, -1, 0; 1, 0, 0; 0, 0, 1], which I think is correct. So I must have a sign wrong somewhere? I don't suppose you'd have any more insights? – Joe Oct 21 '20 at 20:42
• @Joe - It looks like the last line of your DH table is off, I think. You have <θ2, 180, 0, 0> when I think you should have <180+θ2, 180, 0, 0>. Your x- and z-axes are both pointing in opposite directions when you compare the end effector frame and the frame prior. – Chuck Oct 22 '20 at 14:31
• thanks for that - I’d done this in the code but not the table. I’ve updated the table now. I still seem to have something wrong somewhere! – Joe Oct 22 '20 at 15:39
• thanks for your help with this - much clearer way of visualising the problem! – Joe Oct 23 '20 at 20:30