%PUMA3DOF Puma three degrees of freedom robot % takes the three joint angles, calculates the forward kinematics of the % Puma arm. % % plots a stick diagram of the arm along with the link coordinate frames. % % Syntax: puma3dof(theta1, theta2, theta3, drawmode) % % Inputs: % theta1 - joint angle 1 % theta2 - joint angle 2 % theta3 - joint angle 3 % drawmode - A 2-vector controlling display of the arm % drawmode(1) controls type of robot representation that is displayed % 0 - Nothing. % 1 - Stick diagram of robot. % 2 - Rendered `cylinder' representation of robot. % drawmode(2) controls whether coordinate frames are displayed % 0 - No frames displayed. % 1 - Coordinate frames are displayed % % Other m-files required: plotframe.m, DHtrans.m, cosmetics.m, % closedcylinder.m, invht.m, gripper.m % % See also: STANFORD3DOF % Author: Travis Hydzik % Last revision: 19 October 2004 function puma3dof(theta1, theta2, theta3, drawmode) % DH parameters d1 = 3; l2 = 3; l3 = 3; % theta, offset, length, twist DH = [ theta1 d1 0 pi/2 theta2 0 l2 0 theta3 0 l3 0 ]; % define initial origin links(:,:,1) = trans(0, 0, 0); % allow for arbitrary DH parameters n = size(DH, 1); % calculate each joint transform and store joint position for i = 1:n T = DHtrans(DH(i, 1), DH(i, 2), DH(i, 3), DH(i, 4)); links(:,:,i+1) = links(:,:,i)*invht(links(:,:,1))*T*links(:,:,1); end % radius of links r = 0.3; hold on; % drawmodes for i = 1:n+1 if drawmode(2) plotframe(links(:,:,i)); end if drawmode(1) == 1 && i <= n 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)]); elseif drawmode(1) == 2 && i <= n [X, Y, Z] = closedCylinder(links(:,4,i), links(:,4,i+1), r); surfl(X,Y,Z); if i < n [X, Y, Z] = closedCylinder(links(:,:,i+1)*[0 0 (r+r/10) 1]', links(:,:,i+1)*[0 0 -(r+r/10) 1]', r); surfl(X,Y,Z); end end end hold off; cosmetics(7);