%SerialLink.ikine3 Inverse kinematics for 3-axis robot with no wrist % % Q = R.ikine3(T) is the joint coordinates corresponding to the robot % end-effector pose T represented by the homogenenous transform. This % is a analytic solution for a 3-axis robot (such as the first three joints % of a robot like the Puma 560). % % Q = R.ikine3(T, CONFIG) as above but specifies the configuration of the arm in % the form of a string containing one or more of the configuration codes: % % 'l' arm to the left (default) % 'r' arm to the right % 'u' elbow up (default) % 'd' elbow down % % Notes:: % - The same as IKINE6S without the wrist. % - The inverse kinematic solution is generally not unique, and % depends on the configuration string. % - Joint offsets, if defined, are added to the inverse kinematics to % generate Q. % % Reference:: % % Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang % From The International Journal of Robotics Research % Vol. 5, No. 2, Summer 1986, p. 32-44 % % % Author:: % Robert Biro with Gary Von McMurray, % GTRI/ATRP/IIMB, % Georgia Institute of Technology % 2/13/95 % % See also SerialLink.FKINE, SerialLink.IKINE. function theta = ikine3(robot, T, varargin) if ~strncmp(robot.config, 'RRR', 3) error('Solution only applicable for 6DOF all-revolute manipulator'); end if robot.mdh ~= 0 error('Solution only applicable for standard DH conventions'); end if ndims(T) == 3 theta = zeros(size(T,3),robot.n); for k=1:size(T,3) theta(k,:) = ikine3(robot, T(:,:,k), varargin{:}); end return; end L = robot.links; a2 = L(2).a; a3 = L(3).a; d3 = L(3).d; if ~ishomog(T) error('T is not a homog xform'); end % undo base transformation T = robot.base \ T; % The following parameters are extracted from the Homogeneous % Transformation as defined in equation 1, p. 34 Px = T(1,4); Py = T(2,4); Pz = T(3,4); % The configuration parameter determines what n1,n2 values are used % and how many solutions are determined which have values of -1 or +1. if nargin < 3 configuration = ''; else configuration = lower(varargin{1}); end % default configuration n1 = -1; % L n2 = -1; % U if ~isempty(strfind(configuration, 'l')) n1 = -1; end if ~isempty(strfind(configuration, 'r')) n1 = 1; end if ~isempty(strfind(configuration, 'u')) if n1 == 1 n2 = 1; else n2 = -1; end end if ~isempty(strfind(configuration, 'd')) if n1 == 1 n2 = -1; else n2 = 1; end end % % Solve for theta(1) % % r is defined in equation 38, p. 39. % theta(1) uses equations 40 and 41, p.39, % based on the configuration parameter n1 % r=sqrt(Px^2 + Py^2); if (n1 == 1) theta(1)= atan2(Py,Px) + asin(d3/r); else theta(1)= atan2(Py,Px) + pi - asin(d3/r); end % % Solve for theta(2) % % V114 is defined in equation 43, p.39. % r is defined in equation 47, p.39. % Psi is defined in equation 49, p.40. % theta(2) uses equations 50 and 51, p.40, based on the configuration % parameter n2 % V114= Px*cos(theta(1)) + Py*sin(theta(1)); r=sqrt(V114^2 + Pz^2); Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r)); if ~isreal(Psi) warning('RTB:ikine3:notreachable', 'point not reachable'); theta = [NaN NaN NaN NaN NaN NaN]; return end theta(2) = atan2(Pz,V114) + n2*Psi; % % Solve for theta(3) % % theta(3) uses equation 57, p. 40. % num = cos(theta(2))*V114+sin(theta(2))*Pz-a2; den = cos(theta(2))*Pz - sin(theta(2))*V114; theta(3) = atan2(a3,d4) - atan2(num, den); % remove the link offset angles for i=1:robot.n %#ok<*AGROW> theta(i) = theta(i) - L(i).offset; end