%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_Viper_S1700D(robot, T)
Solves the inverse kinematic problem for the ADEPT Viper_S1700D 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_Viper_S1700D returns 8 possible solutions, thus,
Q is a 6x8 matrix where each column stores 6 feasible joint values.
Example code:
robot=load_robot('ADEPT', 'Viper_s1700D');
q = [0 0 0 0 0 0];
T = directkinematic(robot, q);
%Call the inversekinematic for this robot
qinv = inversekinematic(robot, T);
check that all of them are feasible solutions!
and every Ti equals T
for i=1:8,
Ti = directkinematic(robot, qinv(:,i))
end
See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q = INVERSEKINEMATIC_Viper_S1700D(robot, T) 0003 % Solves the inverse kinematic problem for the ADEPT Viper_S1700D 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_Viper_S1700D 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 % robot=load_robot('ADEPT', 'Viper_s1700D'); 0016 % q = [0 0 0 0 0 0]; 0017 % T = directkinematic(robot, q); 0018 % %Call the inversekinematic for this robot 0019 % qinv = inversekinematic(robot, T); 0020 % check that all of them are feasible solutions! 0021 % and every Ti equals T 0022 % for i=1:8, 0023 % Ti = directkinematic(robot, qinv(:,i)) 0024 % end 0025 % See also DIRECTKINEMATIC. 0026 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0027 0028 % Copyright (C) 2012, by Arturo Gil Aparicio 0029 % 0030 % This file is part of ARTE (A Robotics Toolbox for Education). 0031 % 0032 % ARTE is free software: you can redistribute it and/or modify 0033 % it under the terms of the GNU Lesser General Public License as published by 0034 % the Free Software Foundation, either version 3 of the License, or 0035 % (at your option) any later version. 0036 % 0037 % ARTE is distributed in the hope that it will be useful, 0038 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0039 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0040 % GNU Lesser General Public License for more details. 0041 % 0042 % You should have received a copy of the GNU Leser General Public License 0043 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0044 function [q] = inversekinematic_Viper_s1700D(robot, T) 0045 0046 %initialize q, 0047 %eight possible solutions are generally feasible 0048 q=zeros(6,8); 0049 0050 %Evaluate the parameters 0051 theta = eval(robot.DH.theta); 0052 d = eval(robot.DH.d); 0053 a = eval(robot.DH.a); 0054 alpha = eval(robot.DH.alpha); 0055 0056 %See geometry at the reference for this robot, distance from the wrist to 0057 %the end effector 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, obtain theta1 0076 % by geometric methods 0077 q1 = -atan2(Pm(1), Pm(2)); 0078 0079 0080 %solve for q2 0081 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm); 0082 0083 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm); 0084 0085 %solve for q3 0086 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm); 0087 0088 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm); 0089 0090 0091 %Arrange solutions, there are 8 possible solutions so far. 0092 % if q1 is a solution, q1* = q1 + pi is also a solution. 0093 % For each (q1, q1*) there are two possible solutions 0094 % for q2 and q3 (namely, elbow up and elbow up solutions) 0095 % So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3), 0096 % there exist two more possible solutions for the last three joints, generally 0097 % called wrist up and wrist down solutions. For this reason, 0098 %the next matrix doubles each column. For each two columns, two different 0099 %configurations for theta4, theta5 and theta6 will be computed. These 0100 %configurations are generally referred as wrist up and wrist down solution 0101 q = [q1 q1 q1 q1 q1+pi q1+pi q1+pi q1+pi; 0102 q2_1(1) q2_1(1) q2_1(2) q2_1(2) q2_2(1) q2_2(1) q2_2(2) q2_2(2); 0103 q3_1(1) q3_1(1) q3_1(2) q3_1(2) q3_2(1) q3_2(1) q3_2(2) q3_2(2); 0104 0 0 0 0 0 0 0 0; 0105 0 0 0 0 0 0 0 0; 0106 0 0 0 0 0 0 0 0]; 0107 0108 %leave only the real part of the solutions 0109 q=real(q); 0110 0111 %Note that in this robot, the joint q3 has a non-simmetrical range. In this 0112 %case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing 0113 %step is avoided in this angle (the next line is commented). When solving 0114 %for the orientation, the solutions are normalized to the [-pi, pi] range 0115 %only for the theta4, theta5 and theta6 joints. 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_wrist(robot, q(:,i), T, 1, 'geometric'); %wrist up 0132 qtemp = solve_spherical_wrist(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_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist down 0137 qtemp = solve_spherical_wrist(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 % 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 theta = eval(robot.DH.theta); 0153 d = eval(robot.DH.d); 0154 a = eval(robot.DH.a); 0155 alpha = eval(robot.DH.alpha); 0156 0157 %DH table parameters with which we calculate theta2, using 0158 %geometric methods 0159 L2=a(2); 0160 L3=d(4); 0161 0162 %Offset distance between the centers of the reference systems of the links 0163 %2 and 3 0164 A1 = a(3); 0165 0166 %See geometry of the robot. Considering L4 calculate the offset between the 0167 %centers of the reference systems of the links 2 and 3 0168 L4 = sqrt(A1^2 + L3^2); 0169 0170 %given q1 is known, compute first DH transformation 0171 T01=dh(robot, q, 1); 0172 0173 %Express Pm in the reference system 1, for convenience 0174 p1 = inv(T01)*[Pm; 1]; 0175 0176 %Distance between the system 1 to the wrist 0177 r = sqrt(p1(1)^2 + p1(2)^2); 0178 0179 beta = atan2(-p1(2), p1(1)); 0180 gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2))); %Theorem of the cosine 0181 0182 if ~isreal(gamma) 0183 disp('WARNING:inversekinematic_Viper_s1700D: the point is not reachable for this configuration, imaginary solutions'); 0184 %gamma = real(gamma); 0185 end 0186 0187 %return two possible solutions 0188 %elbow up and elbow down 0189 %the order here is important and is coordinated with the function 0190 %solve_for_theta2. We add pi/2 to offset lags in our reference systems 0191 q2(1) = pi/2 - beta - gamma; %elbow up 0192 q2(2) = pi/2 - beta + gamma; %elbow down 0193 0194 0195 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0196 % solve for third joint theta3, two different 0197 % solutions are returned, corresponding 0198 % to elbow up and down solution 0199 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0200 function q3 = solve_for_theta3(robot, q, Pm) 0201 0202 %Evaluate the parameters 0203 theta = eval(robot.DH.theta); 0204 d = eval(robot.DH.d); 0205 a = eval(robot.DH.a); 0206 alpha = eval(robot.DH.alpha); 0207 0208 %DH table parameters with which we calculate theta3, using 0209 %geometric methods 0210 L2=a(2); 0211 L3=d(4); 0212 0213 A1 = a(3); 0214 0215 %See geometry of the robot, like in the function q2 0216 L4 = sqrt(A1^2 + L3^2); 0217 0218 %The delta angle is fixed because they are the lines that make up the gap 0219 %between the links 2, 3 and 4 0220 delta = real(acos((A1^2+L4^2-L3^2)/(2*A1*L4))); %Theorem of the cosine 0221 0222 %given q1 is known, compute first DH transformation 0223 T01=dh(robot, q, 1); 0224 0225 %Express Pm in the reference system 1, for convenience 0226 p1 = inv(T01)*[Pm; 1]; 0227 0228 %Same as the function q2 0229 r = sqrt(p1(1)^2 + p1(2)^2); 0230 0231 %Real angle between the links 2 and 3 0232 ro = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); %Theorem of the cosine 0233 0234 if ~isreal(ro) 0235 disp('WARNING:inversekinematic_Viper_s1700D: the point is not reachable for this configuration, imaginary solutions'); 0236 %ro = real(ro); 0237 end 0238 0239 %return two possible solutions 0240 %elbow up and elbow down 0241 %the order here is important and is coordinated with the function 0242 %solve_for_theta3. We add pi to offset lags in our reference systems 0243 q3(1) = pi - ro - delta; %elbow up 0244 q3(2) = pi + ro - delta; %elbow down 0245 0246 0247 0248 %remove complex solutions for q for the q1+pi solutions 0249 function qreal = arrange_solutions(q) 0250 qreal=q(:,1:4); 0251 0252 %sum along rows if any angle is complex, for any possible solutions, then v(i) is complex 0253 v = sum(q, 1); 0254 0255 for i=5:8, 0256 if isreal(v(i)) 0257 qreal=[qreal q(:,i)]; %store the real solutions 0258 end 0259 end 0260 0261 qreal = real(qreal);