Forward Kinematics for Two Axis Gimbal Mount

Question

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?

Answer 1

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