Description of inversekinematic_irb1600_6

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %   Q = INVERSEKINEMATIC_IRB1600_6_120(robot, T)
0003 %   Solves the inverse kinematic problem for the ABB IRB 1600 6/120 robot
0004 %   where:
0005 %   robot stores the robot parameters.
0006 %   T is an homogeneous transform that specifies the position/orientation
0007 %   of the end effector.
0008 %
0009 %   A call to Q=INVERSEKINEMATIC_IRB1600_6_120 returns 8 possible solutions, thus,
0010 %   Q is a 6x8 matrix where each column stores 6 feasible joint values.
0011 %
0012 %
0013 %   Example code:
0014 %
0015 %   abb=load_robot('ABB', 'IRB1600_6_120');
0016 %   q = [0 0 0 0 0 0];
0017 %   T = directkinematic(abb, q);
0018 %   %Call the inversekinematic for this robot
0019 %   qinv = inversekinematic(abb, T);
0020 %   check that all of them are feasible solutions!
0021 %   and every Ti equals T
0022 %
0023 %   for i=1:size(qinv,2),
0024 %      Ti=directkinematic(robot,qinv(:,i))
0025 %      test_joints(robot,qinv(:,i))
0026 %      drawrobot3d(robot,qinv(:,i))
0027 %      pause(1)
0028 %   end
0029 %
0030 %    See also DIRECTKINEMATIC.
0031 %
0032 %   Author: Arturo Gil Aparicio
0033 %           Universitas Miguel Hernandez, SPAIN.
0034 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0035 
0036 % Copyright (C) 2012, by Arturo Gil Aparicio
0037 %
0038 % This file is part of ARTE (A Robotics Toolbox for Education).
0039 %
0040 % ARTE is free software: you can redistribute it and/or modify
0041 % it under the terms of the GNU Lesser General Public License as published by
0042 % the Free Software Foundation, either version 3 of the License, or
0043 % (at your option) any later version.
0044 %
0045 % ARTE is distributed in the hope that it will be useful,
0046 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0047 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0048 % GNU Lesser General Public License for more details.
0049 %
0050 % You should have received a copy of the GNU Leser General Public License
0051 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0052 function [q, valid] = inversekinematic_irb1600_6_120(robot, T)
0053 
0054 %initialize q,
0055 %eight possible solutions are generally feasible
0056 q=zeros(6,8);
0057 
0058 %Evaluate the parameters
0059 %theta = eval(robot.DH.theta);
0060 d = eval(robot.DH.d);
0061 a = eval(robot.DH.a);
0062 %alpha = eval(robot.DH.alpha);
0063 
0064 
0065 %See geometry at the reference for this robot
0066 %L1=d(1);
0067 %L2=a(2);
0068 %L3=d(4);
0069 L6=d(6);
0070 
0071 %A1 = a(1);
0072 
0073 
0074 %T= [ nx ox ax Px;
0075 %     ny oy ay Py;
0076 %     nz oz az Pz];
0077 Px=T(1,4);
0078 Py=T(2,4);
0079 Pz=T(3,4);
0080 
0081 %Compute the position of the wrist, being W the Z component of the end effector's system
0082 W = T(1:3,3);
0083 
0084 % Pm: wrist position
0085 Pm = [Px Py Pz]' - L6*W; 
0086 
0087 %first joint, two possible solutions admited:
0088 % if q(1) is a solution, then q(1) + pi is also a solution
0089 q1=atan2(Pm(2), Pm(1));
0090 
0091 
0092 %solve for q2
0093 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
0094 
0095 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);
0096 
0097 %solve for q3
0098 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
0099 
0100 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);
0101 
0102 
0103 %Arrange solutions, there are 8 possible solutions so far.
0104 % if q1 is a solution, q1* = q1 + pi is also a solution.
0105 % For each (q1, q1*) there are two possible solutions
0106 % for q2 and q3 (namely, elbow up and elbow up solutions)
0107 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
0108 % there exist two more possible solutions for the last three joints, generally
0109 % called wrist up and wrist down solutions. For this reason,
0110 %the next matrix doubles each column. For each two columns, two different
0111 %configurations for theta4, theta5 and theta6 will be computed. These
0112 %configurations are generally referred as wrist up and wrist down solution
0113 q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;   
0114      q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
0115      q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
0116      0          0          0         0         0      0       0       0;
0117      0          0          0         0         0      0       0       0;
0118      0          0          0         0         0      0       0       0];
0119 
0120 %leave only the real part of the solutions
0121 q=real(q);
0122 
0123 %normalize q to [-pi, pi]
0124 q(1,:) = normalize(q(1,:));
0125 q(2,:) = normalize(q(2,:));
0126 
0127 % solve for the last three joints
0128 % for any of the possible combinations (theta1, theta2, theta3)
0129 for i=1:2:size(q,2),
0130     %Use solve_spherical_wrist2 for the ABB robots, since the special
0131     %initial orientation needs to be matched
0132     qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up
0133     qtemp(4:6)=normalize(qtemp(4:6));
0134     q(:,i)=qtemp;
0135     
0136     qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down
0137     qtemp(4:6)=normalize(qtemp(4:6));
0138     q(:,i+1)=qtemp;
0139 end
0140 
0141 
0142  
0143 
0144 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0145 % solve for second joint theta2, two different
0146 % solutions are returned, corresponding
0147 % to elbow up and down solution
0148 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0149 function q2 = solve_for_theta2(robot, q, Pm)
0150 
0151 %Evaluate the parameters
0152 d = eval(robot.DH.d);
0153 a = eval(robot.DH.a);
0154 
0155 %See geometry
0156 L2=a(2);
0157 L3=d(4);
0158 
0159 %given q1 is known, compute first DH transformation
0160 T01=dh(robot, q, 1);
0161 
0162 %Express Pm in the reference system 1, for convenience
0163 p1 = inv(T01)*[Pm; 1];
0164 
0165 r = sqrt(p1(1)^2 + p1(2)^2);
0166 
0167 beta = atan2(-p1(2), p1(1));
0168 gamma = (acos((L2^2+r^2-L3^2)/(2*r*L2)));
0169 
0170 if ~isreal(gamma)
0171     disp('WARNING:inversekinematic_irb1600_6-120: the point is not reachable for this configuration, imaginary solutions'); 
0172     %gamma = real(gamma);
0173 end
0174 
0175 %return two possible solutions
0176 %elbow up and elbow down
0177 %the order here is important and is coordinated with the function
0178 %solve_for_theta3
0179 q2(1) = pi/2 - beta - gamma; %elbow up
0180 q2(2) = pi/2 - beta + gamma; %elbow down
0181 
0182 
0183 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0184 % solve for third joint theta3, two different
0185 % solutions are returned, corresponding
0186 % to elbow up and down solution
0187 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0188 function q3 = solve_for_theta3(robot, q, Pm)
0189 
0190 %Evaluate the parameters
0191 d = eval(robot.DH.d);
0192 a = eval(robot.DH.a);
0193 
0194 %See geometry
0195 L2=a(2);
0196 L3=d(4);
0197 
0198 %given q1 is known, compute first DH transformation
0199 T01=dh(robot, q, 1);
0200 
0201 %Express Pm in the reference system 1, for convenience
0202 p1 = inv(T01)*[Pm; 1];
0203 
0204 r = sqrt(p1(1)^2 + p1(2)^2);
0205 
0206 eta = (acos((L2^2 + L3^2 - r^2)/(2*L2*L3)));
0207 
0208 if ~isreal(eta)
0209    disp('WARNING:inversekinematic_irb1600_6-120: the point is not reachable for this configuration, imaginary solutions'); 
0210    %eta = real(eta);
0211 end
0212 
0213 %return two possible solutions
0214 %elbow up and elbow down solutions
0215 %the order here is important
0216 q3(1) = pi/2 - eta;
0217 q3(2) = eta - 3*pi/2;
0218 
0219 
0220 
0221 
0222
inversekinematic_irb1600_6_120

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SUBFUNCTIONS

SOURCE CODE
