Description of inversekinematic_IRB_7600

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %   Q = INVERSEKINEMATIC_IRB_7600_150(robot, T)
0003 %   Solves the inverse kinematic problem for the ABB IRB 7600_150 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_IRB_7600_150 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', 'IRB_7600_150');
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 %   for i=1:8,
0023 %        Ti = directkinematic(abb, qinv(:,i))
0024 %   end
0025 %
0026 %    See also DIRECTKINEMATIC.
0027 %
0028 %   Author: Alba Pastor Roca, Patricia Vicente Coves, Javier Cupeiro
0029 %   Martinezn, David Lopez Ibanez.
0030 %           Universitas Miguel Hernandez, SPAIN.
0031 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0032 
0033 % Copyright (C) 2012, by Arturo Gil Aparicio
0034 %
0035 % This file is part of ARTE (A Robotics Toolbox for Education).
0036 %
0037 % ARTE is free software: you can redistribute it and/or modify
0038 % it under the terms of the GNU Lesser General Public License as published by
0039 % the Free Software Foundation, either version 3 of the License, or
0040 % (at your option) any later version.
0041 %
0042 % ARTE is distributed in the hope that it will be useful,
0043 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0044 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0045 % GNU Lesser General Public License for more details.
0046 %
0047 % You should have received a copy of the GNU Leser General Public License
0048 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0049 function q = inversekinematic_IRB_7600_150(robot, T)
0050 
0051 %initialize q,
0052 %eight possible solutions are generally feasible
0053 q=zeros(6,8);
0054 
0055 % %Evaluate the parameters
0056 % theta = eval(robot.DH.theta);
0057 d = eval(robot.DH.d);
0058 L6=d(6);
0059 
0060 
0061 %T= [ nx ox ax Px;
0062 %     ny oy ay Py;
0063 %     nz oz az Pz];
0064 Px=T(1,4);
0065 Py=T(2,4);
0066 Pz=T(3,4);
0067 
0068 %Compute the position of the wrist, being W the Z component of the end effector's system
0069 W = T(1:3,3);
0070 
0071 % Pm: wrist position
0072 Pm = [Px Py Pz]' - L6*W; 
0073 
0074 %first joint, two possible solutions admited:
0075 % if q(1) is a solution, then q(1) + pi is also a solution
0076 q1=atan2(Pm(2), Pm(1));
0077 
0078 
0079 %solve for q2
0080 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
0081 %the other possible solution is q1 + pi
0082 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);
0083 
0084 %solve for q3
0085 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
0086 %solver for q3 for both cases
0087 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);
0088 
0089 
0090 %the next matrix doubles each column. For each two columns, two different
0091 %configurations for theta4, theta5 and theta6 will be computed. These
0092 %configurations are generally referred as wrist up and wrist down solution
0093 q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;   
0094      q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
0095      q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
0096      0          0          0         0         0      0       0       0;
0097      0          0          0         0         0      0       0       0;
0098      0          0          0         0         0      0       0       0];
0099 
0100  %leave only the real part of the solutions
0101 q=real(q);
0102 
0103 %Note that in this robot, the joint q3 has a non-simmetrical range. In this
0104 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
0105 %step is avoided in this angle (the next line is commented). When solving
0106 %for the orientation, the solutions are normalized to the [-pi, pi] range
0107 %only for the theta4, theta5 and theta6 joints.
0108 
0109 %normalize q to [-pi, pi]
0110 q(1,:) = normalize(q(1,:));
0111 q(2,:) = normalize(q(2,:));
0112 
0113 % solve for the last three joints
0114 % for any of the possible combinations (theta1, theta2, theta3)
0115 for i=1:2:size(q,2),
0116     % use solve_spherical_wrist2 for the particular orientation
0117     % of the systems in this ABB robot
0118     % use either the geometric or algebraic method.
0119     % the function solve_spherical_wrist2 is used due to the relative
0120     % orientation of the last three DH reference systems.
0121     
0122     %use either one algebraic method or the geometric
0123     %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up
0124     qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up
0125     qtemp(4:6)=normalize(qtemp(4:6));
0126     q(:,i)=qtemp;
0127     
0128     %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down
0129     qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down
0130     qtemp(4:6)=normalize(qtemp(4:6));
0131     q(:,i+1)=qtemp;
0132 end
0133 
0134 
0135 
0136 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0137 % solve for second joint theta2, two different
0138 % solutions are returned, corresponding
0139 % to elbow up and down solution
0140 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0141 function q2 = solve_for_theta2(robot, q, Pm)
0142 
0143 %Evaluate the parameters
0144 theta = eval(robot.DH.theta);
0145 d = eval(robot.DH.d);
0146 a = eval(robot.DH.a);
0147 alpha = eval(robot.DH.alpha);
0148 
0149 %See geometry
0150 L2=a(2);
0151 L3=d(4);
0152 A2=a(3);
0153 
0154 %See geometry of the robot
0155 %compute L4
0156 L4 = sqrt(A2^2 + L3^2);
0157 
0158 %The inverse kinematic problem can be solved as in the IRB 140 (for example)
0159 
0160 %given q1 is known, compute first DH transformation
0161 T01=dh(robot, q, 1);
0162 
0163 %Express Pm in the reference system 1, for convenience
0164 p1 = inv(T01)*[Pm; 1];
0165 
0166 r = sqrt(p1(1)^2 + p1(2)^2);
0167 
0168 beta = atan2(-p1(2), p1(1));
0169 gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));
0170 
0171 %return two possible solutions
0172 %elbow up and elbow down
0173 %the order here is important and is coordinated with the function
0174 %solve_for_theta3
0175 q2(1) = pi/2 - beta - gamma; %elbow up
0176 q2(2) = pi/2 - beta + gamma; %elbow down
0177 
0178 
0179 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0180 % solve for third joint theta3, two different
0181 % solutions are returned, corresponding
0182 % to elbow up and down solution
0183 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0184 function q3 = solve_for_theta3(robot, q, Pm)
0185 
0186 %Evaluate the parameters
0187 theta = eval(robot.DH.theta);
0188 d = eval(robot.DH.d);
0189 a = eval(robot.DH.a);
0190 alpha = eval(robot.DH.alpha);
0191 
0192 %See geometry
0193 L2=a(2);
0194 L3=d(4);
0195 A2=a(3);
0196 
0197 %See geometry of the robot
0198 %compute L4
0199 L4 = sqrt(A2^2 + L3^2);
0200 
0201 %the angle phi is fixed
0202 phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));
0203 
0204 %given q1 is known, compute first DH transformation
0205 T01=dh(robot, q, 1);
0206 
0207 %Express Pm in the reference system 1, for convenience
0208 p1 = inv(T01)*[Pm; 1];
0209 
0210 r = sqrt(p1(1)^2 + p1(2)^2);
0211 
0212 eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));
0213 
0214 %return two possible solutions
0215 %elbow up and elbow down solutions
0216 %the order here is important
0217 q3(1) = pi - phi- eta; 
0218 q3(2) = pi - phi + eta; 
0219
inversekinematic_IRB_7600_150

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SUBFUNCTIONS

SOURCE CODE
