Home > arte3.2.0 > robots > ABB > IRB1600ID > inversekinematic_irb1600id.m

inversekinematic_irb1600id

PURPOSE ^

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

SYNOPSIS ^

function q = inversekinematic_irb1600id(robot, T)

DESCRIPTION ^

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   Q = INVERSEKINEMATIC_IRB1600id(robot, T)    
   Solves the inverse kinematic problem for the ABB IRB 140 robot
   where:
   robot stores the robot parameters.
   T is an homogeneous transform that specifies the position/orientation
   of the end effector.

   A call to Q=INVERSEKINEMATIC_IRB1600ID returns 8 possible solutions, thus,
   Q is a 6x8 matrix where each column stores 6 feasible joint values.

   
   Example code:

   abb=load_robot('abb', 'IRB1600ID');
   q = [0 0 0 0 0 0];    
   T = directkinematic(abb, q);
   %Call the inversekinematic for this robot
   qinv = inversekinematic(abb, T);
   check that all of them are feasible solutions!
   and every Ti equals T
   for i=1:8,
        Ti = directkinematic(abb, qinv(:,i))
   end
    See also DIRECTKINEMATIC.

   Author: Daniel Vivancos Unica
           Jose David Martinez Exposito,
           Maria Jose Martinez Liza

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %   Q = INVERSEKINEMATIC_IRB1600id(robot, T)
0003 %   Solves the inverse kinematic problem for the ABB IRB 140 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_IRB1600ID 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', 'IRB1600ID');
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 %    See also DIRECTKINEMATIC.
0026 %
0027 %   Author: Daniel Vivancos Unica
0028 %           Jose David Martinez Exposito,
0029 %           Maria Jose Martinez Liza
0030 %
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_irb1600id(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 a = eval(robot.DH.a);
0059 alpha = eval(robot.DH.alpha);
0060 
0061 
0062 %See geometry at the reference for this robot
0063 L6=d(6);
0064 
0065 
0066 %T= [ nx ox ax Px;
0067 %     ny oy ay Py;
0068 %     nz oz az Pz];
0069 Px=T(1,4);
0070 Py=T(2,4);
0071 Pz=T(3,4);
0072 
0073 %Compute the position of the wrist, being W the Z component of the end effector's system
0074 W = T(1:3,3);
0075 
0076 % Pm: wrist position
0077 Pm = [Px Py Pz]' - L6*W; 
0078 
0079 %first joint, two possible solutions admited:
0080 % if q(1) is a solution, then q(1) + pi is also a solution
0081 q1=atan2(Pm(2), Pm(1));
0082 
0083 
0084 %solve for q2
0085 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
0086 
0087 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);
0088 
0089 %solve for q3
0090 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
0091 
0092 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);
0093 
0094 
0095 %Arrange solutions, there are 8 possible solutions so far.
0096 % if q1 is a solution, q1* = q1 + pi is also a solution.
0097 % For each (q1, q1*) there are two possible solutions
0098 % for q2 and q3 (namely, elbow up and elbow up solutions)
0099 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
0100 % there exist two more possible solutions for the last three joints, generally
0101 % called wrist up and wrist down solutions. For this reason,
0102 %the next matrix doubles each column. For each two columns, two different
0103 %configurations for theta4, theta5 and theta6 will be computed. These
0104 %configurations are generally referred as wrist up and wrist down solution
0105 q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;   
0106      q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
0107      q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
0108      0          0          0         0         0      0       0       0;
0109      0          0          0         0         0      0       0       0;
0110      0          0          0         0         0      0       0       0];
0111 
0112 
0113 %leave only the real part of the solutions
0114 q=real(q);
0115 
0116 
0117 %normalize q to [-pi, pi]
0118 q(1,:) = normalize(q(1,:));
0119 q(2,:) = normalize(q(2,:));
0120 
0121 % solve for the last three joints
0122 % for any of the possible combinations (theta1, theta2, theta3)
0123 for i=1:2:size(q,2),
0124     % use solve_spherical_wrist2 for the particular orientation
0125     % of the systems in this ABB robot
0126     % use either the geometric or algebraic method.
0127     % the function solve_spherical_wrist2 is used due to the relative
0128     % orientation of the last three DH reference systems.
0129     
0130     %use either one algebraic method or the geometric
0131     %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up
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, 'geometric'); %wrist down
0137     qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down
0138     qtemp(4:6)=normalize(qtemp(4:6));
0139     q(:,i+1)=qtemp;
0140 end
0141 
0142 
0143  
0144 
0145 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0146 % solve for second joint theta2, two different
0147 % solutions are returned, corresponding
0148 % to elbow up and down solution
0149 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0150 
0151 function q2 = solve_for_theta2(robot, q, Pm)
0152 
0153 %Evaluate the parameters
0154 d = eval(robot.DH.d);
0155 a = eval(robot.DH.a);
0156 
0157 %See geometry
0158 L2=a(2);
0159 L3=d(4);
0160 
0161 A2=a(3); % desfase porculero
0162 
0163 %given q1 is known, compute first DH transformation
0164 T01=dh(robot, q, 1);
0165 
0166 %brazo ficticio que nos quita el desfase
0167 l3 = sqrt (A2^2 + L3^2);
0168 
0169 %Express Pm in the reference system 1, for convenience
0170 p1 = inv(T01)*[Pm; 1];
0171 
0172 r = sqrt(p1(1)^2 + p1(2)^2);
0173 
0174 beta = atan2(-p1(2), p1(1));
0175 gamma = (acos((L2^2+r^2-l3^2)/(2*r*L2)));
0176 
0177 if ~isreal(gamma)
0178     disp('WARNING:inversekinematic_irb1600id: the point is not reachable for this configuration, imaginary solutions'); 
0179     %gamma = real(gamma);
0180 end
0181 
0182 %return two possible solutions
0183 %elbow up and elbow down
0184 %the order here is important and is coordinated with the function
0185 %solve_for_theta3
0186 q2(1) = pi/2 - beta - gamma; %elbow up
0187 q2(2) = pi/2 - beta + gamma; %elbow down
0188 
0189 
0190 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0191 % solve for third joint theta3, two different
0192 % solutions are returned, corresponding
0193 % to elbow up and down solution
0194 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0195 
0196 function q3 = solve_for_theta3(robot, q, Pm)
0197 
0198 %Evaluate the parameters
0199 d = eval(robot.DH.d);
0200 a = eval(robot.DH.a);
0201 
0202 %See geometry
0203 L2=a(2);
0204 L3=d(4);
0205 
0206 A2= a(3); %desfase
0207 
0208 %See geometry of the robot
0209 %compute L4
0210 l3 = sqrt(A2^2 + L3^2);
0211 
0212 %the angle phi is fixed
0213 phi=acos((A2^2+l3^2-L3^2)/(2*A2*l3));
0214 
0215 %given q1 is known, compute first DH transformation
0216 T01=dh(robot, q, 1);
0217 
0218 %Express Pm in the reference system 1, for convenience
0219 p1 = inv(T01)*[Pm; 1];
0220 
0221 r = sqrt(p1(1)^2 + p1(2)^2);
0222 
0223 eta = (acos((L2^2 + l3^2 - r^2)/(2*L2*l3)));
0224 
0225 if ~isreal(eta)
0226    disp('WARNING:inversekinematic_irb140: the point is not reachable for this configuration, imaginary solutions'); 
0227    %eta = real(eta);
0228 end
0229 
0230 %return two possible solutions
0231 %elbow up and elbow down solutions
0232 %the order here is important
0233 q3(1) = pi - phi - eta;
0234 q3(2) = pi - phi + eta;
0235 
0236

Generated on Fri 03-Jan-2014 12:20:01 by m2html © 2005