%STANFORD3DOF Stanford three degrees of freedom robot % takes the two joint angles and the prismatic extension of the last link, % and calculates the forward kinematics of the Stanford Arm. % % plots a stick diagram of the arm along with the link coordinate frames. % % Syntax: stanford3dof(theta1, theta2, d3) % % Inputs: % theta1 - joint angle 1 % theta2 - joint angle 2 % d3 - prismatic extension of last link % % Other m-files required: plotframe.m, DHtrans.m, cosmetics.m, invht.m % % See also: PUMA3DOF % Author: Travis Hydzik % Last revision: 19 October 2004 function stanford3dof(theta1, theta2, d3) % DH parameters d1 = 2; % theta, offset, length, twist DH = [ theta1 d1 0 pi/2 theta2 0 0 -pi/2 0 d3 0 0 ]; hold on; % define and plotframe initial origin links(:,:,1) = trans(0, 0, 0); plotframe(links(:,:,1)); % calculate each joint transform and store joint position for i = 1:size(DH, 1) % Denavit-Hartenberg transformation T = DHtrans(DH(i, 1), DH(i, 2), DH(i, 3), DH(i, 4)); % store new link links(:,:,i+1) = links(:,:,i)*invht(links(:,:,1))*T*links(:,:,1); % draw links and frames line([links(1,4,i) links(1,4,i+1)], [links(2,4,i) links(2,4,i+1)], [links(3,4,i) links(3,4,i+1)]); plotframe(links(:,:,i+1)); end hold off; cosmetics(3);