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?