%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q = INVERSEKINEMATIC_STANFORD(robot, T)
Solves the inverse kinematic problem for the STANFORD 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_STANFORD returns 4 possible solutions, thus,
Q is a 6x4 matrix where each column stores 6 feasible joint values. Of
the more general 8 possible solutions, only the 4 that are physically
realizable are returned. This is a consequence of the third
translational axis that, because of its construction, does not allow
negative displacements.
Bibliography: The algorithm has been implemented as is and taken
from: "ROBOT ANALYSIS. The mechanics of Serial and Parallel
manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
0-471-32593-7. pages: 104--109.
Example code:
robot=load_robot('example', 'stanford');
q = [0.1 0.1 0.1 0.1 0.1 0.1];
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_STANFORD(robot, T) 0003 % Solves the inverse kinematic problem for the STANFORD 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_STANFORD returns 4 possible solutions, thus, 0010 % Q is a 6x4 matrix where each column stores 6 feasible joint values. Of 0011 % the more general 8 possible solutions, only the 4 that are physically 0012 % realizable are returned. This is a consequence of the third 0013 % translational axis that, because of its construction, does not allow 0014 % negative displacements. 0015 % 0016 % 0017 % Bibliography: The algorithm has been implemented as is and taken 0018 % from: "ROBOT ANALYSIS. The mechanics of Serial and Parallel 0019 % manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN: 0020 % 0-471-32593-7. pages: 104--109. 0021 % 0022 % Example code: 0023 % 0024 % robot=load_robot('example', 'stanford'); 0025 % q = [0.1 0.1 0.1 0.1 0.1 0.1]; 0026 % T = directkinematic(robot, q); 0027 % %Call the inversekinematic for this robot 0028 % qinv = inversekinematic(robot, T); 0029 % check that all of them are feasible solutions! 0030 % and every Ti equals T 0031 % for i=1:8, 0032 % Ti = directkinematic(robot, qinv(:,i)) 0033 % end 0034 % See also DIRECTKINEMATIC. 0035 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0036 0037 % Copyright (C) 2012, by Arturo Gil Aparicio 0038 % 0039 % This file is part of ARTE (A Robotics Toolbox for Education). 0040 % 0041 % ARTE is free software: you can redistribute it and/or modify 0042 % it under the terms of the GNU Lesser General Public License as published by 0043 % the Free Software Foundation, either version 3 of the License, or 0044 % (at your option) any later version. 0045 % 0046 % ARTE is distributed in the hope that it will be useful, 0047 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0048 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0049 % GNU Lesser General Public License for more details. 0050 % 0051 % You should have received a copy of the GNU Leser General Public License 0052 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0053 function q = inversekinematic_stanford(robot, T) 0054 0055 %initialize, eight possible solutions 0056 q=zeros(6,4); 0057 0058 %evaluate robot parameters 0059 d = eval(robot.DH.d); 0060 0061 h=d(6); 0062 g=d(2); 0063 0064 %T= [ nx ox ax Px; 0065 % ny oy ay Py; 0066 % nz oz az Pz]; 0067 Q=T(1:3,4); 0068 %W is the third orientation vector 0069 W = T(1:3,3); 0070 U = T(1:3,1); 0071 0072 %Compute wrist center position P 0073 P = Q - h*W; 0074 0075 %First compute d3 0076 d3 = sqrt(P(1)^2+P(2)^2+(P(3)-d(1))^2-g^2); 0077 0078 if ~isreal(d3) 0079 disp('\nrobots/stanford/inversekinematic_stanford: THE END POINT IS NOT REACHABLE, IMAGINARY SOLUTION'); 0080 end 0081 0082 %solve for theta2, if theta2 is a solution, -theta2 is also a solution 0083 theta2=real(-asin((P(3)-d(1))/d3)+pi/2); 0084 0085 0086 %returns two possible solutions for theta1 0087 [theta1_1, theta1_2]=solve_for_theta1(g, theta2, d3, P); 0088 0089 0090 %arrange all possible solutions so far 0091 q = [theta1_1 theta1_2; 0092 theta2 -theta2; 0093 d3 d3; 0094 0 0; 0095 0 0; 0096 0 0]; 0097 0098 %solve for the last three joints 0099 %given each of the latter solutions for (theta1, theta2), 0100 %two possible solutions are feasible, namely, wrist up and 0101 % wrist up 0102 q1_1 = solve_for_last_three_joints(robot, q(:,1), T, 1); 0103 q1_2 = solve_for_last_three_joints(robot, q(:,1), T, -1); 0104 0105 q2_1 = solve_for_last_three_joints(robot, q(:,2), T, 1); 0106 q2_2 = solve_for_last_three_joints(robot, q(:,2), T, -1); 0107 0108 0109 %arrange all possible solutions so far 0110 %now, there are 4 possible solutions 0111 q = [theta1_1 theta1_1 theta1_2 theta1_2; 0112 theta2 theta2 -theta2 -theta2; 0113 d3 d3 d3 d3 ; 0114 q1_1(4) q1_2(4) q2_1(4) q2_2(4); 0115 q1_1(5) q1_2(5) q2_1(5) q2_2(5) ; 0116 q1_1(6) q1_2(6) q2_1(6) q2_2(6)]; 0117 0118 0119 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0120 % solve for first joint theta1, given theta2 and d3 0121 % a single solution for theta1 is possible 0122 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0123 function [theta1_1, theta1_2]=solve_for_theta1(g, theta2, d3, P) 0124 0125 H = sqrt(P(1)^2+P(2)^2); 0126 gamma = acos(g/H); 0127 alf = atan2(P(2),P(1)); 0128 theta1_1 = alf+gamma-pi/2; 0129 0130 theta1_2 = alf-gamma-pi/2; 0131 0132 0133 % Solve for the last three joints asuming an spherical wrist 0134 function q = solve_for_last_three_joints(robot, q, T, wrist) 0135 %T= [ nx ox ax Px; 0136 % ny oy ay Py; 0137 % nz oz az Pz]; 0138 Z=T(1:3,3); 0139 X=T(1:3,1); % X orientation vector of the end effector 0140 0141 0142 % Obtain the position and orientation of the system 3 0143 % using the already computed joints q1, q2 and q3 0144 T01=dh(robot, q, 1); 0145 T12=dh(robot, q, 2); 0146 T23=dh(robot, q, 3); 0147 T03=T01*T12*T23; 0148 0149 vx3=T03(1:3,1); 0150 vy3=T03(1:3,2); 0151 vz3=T03(1:3,3); 0152 0153 0154 %find z4 normal to the plane formed by z3 and a 0155 z4=cross(vz3, Z); % end effector's vector a: T(1:3,3) 0156 z4=z4/norm(z4); %normalize 0157 0158 if (sum(z4)==0) | (z4==NaN) 0159 %degenerate case 0160 %if this is the case, a DOF is lost, we choose arbitrarily q4=0 0161 % and later compute q6 to find a suitable solution for the orientation 0162 q(4)=0; 0163 else 0164 cosq4=wrist*dot(z4,vy3); 0165 sinq4=wrist*dot(z4,-vx3); 0166 q(4)=atan2(sinq4, cosq4); 0167 end 0168 0169 % solve for q5 0170 T34=dh(robot, q, 4); 0171 T04=T03*T34; 0172 vx4=T04(1:3,1); 0173 vy4=T04(1:3,2); 0174 %find q5 as the angle formed by Z in the plane of x4 and y4 0175 % Z is coincident with z5 0176 cosq5=dot(Z,-vy4); 0177 sinq5=dot(Z,vx4); 0178 q(5)=atan2(sinq5, cosq5); 0179 0180 % solve for q6 0181 T45=dh(robot, q, 5); 0182 T05=T04*T45; 0183 vx5=T05(1:3,1); 0184 vy5=T05(1:3,2); 0185 cosq6=dot(X,vx5); 0186 sinq6=dot(X,vy5); % Vector de orientaci�n n: T(1:3,1) 0187 q(6)=atan2(sinq6, cosq6); 0188