%INVPUMA6DOF Inverse Puma 6 Degrees of Freedom % Function for calculating the inverse kinematics of a full 6 DOF puma arm % % Syntax: theta = invpuma6dof(T, param) % % Inputs: % T - The homogeneous transform that describes the end-effector frame. % param - [d1 l2 d4 d6] (suggested values [2 2 2 .2]) % The non-zero lengths and offsets. % % Outputs: % theta - a 4x6 element array of joint angles. (up to four different % solutions, each consisting of six joint angles. Note: ignore % the extra 4 solutions obtained by adding pi to theta1) % % Other m-files required: trans.m, DHtrans.m, invht.m, inveuler.m % % See also: PUMA6DOF % Author: Travis Hydzik % Last revision: 20 October 2004 function theta = invpuma6dof(T, param) % solve for the wrist centre T = T*trans(0, 0, -param(4)); theta(1:4,1) = atan2(T(2,4), T(1,4)); rSquared = T(1,4)^2 + T(2,4)^2; s = T(3,4) - param(1); D = (rSquared + s^2 - param(2)^2 - param(3)^2)/(2*param(2)*param(3)); theta(1:2,3) = acos(D) - pi/2; theta(3:4,3) = -acos(D) - pi/2; theta(1:4,2) = atan2(s, sqrt(rSquared)) - asin(param(3)*cos(theta(1:4,3))/sqrt(rSquared + s^2)); for i = 1:2:3 % theta, offset, length, twist DH = [ theta(i,1) param(1) 0 pi/2 theta(i,2) 0 param(2) 0 theta(i,3) 0 0 -pi/2 ]; t = DHtrans(DH(1, 1), DH(1, 2), DH(1, 3), DH(1, 4))*DHtrans(DH(2, 1), DH(2, 2), DH(2, 3), DH(2, 4))*DHtrans(DH(3, 1), DH(3, 2), DH(3, 3), DH(3, 4)); T = invht(t)*T; % solve for inverse orientation [euler1, euler2] = inveuler(T); theta(i:i+1,4:6) = [euler1; euler2]; end