%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q = solve_spherical_wrist(robot, q, T, wrist)
Solves the inverse kinematic problem for a spherical wrist
robot: robot structure.
q: vector containing the values of the joints 1, 2 and 3.
T: orientation of the last reference system.
wrist: select -1 or 1 for two possible solutions (wrist up, wrist down)
See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % q = solve_spherical_wrist(robot, q, T, wrist) 0003 % Solves the inverse kinematic problem for a spherical wrist 0004 % robot: robot structure. 0005 % q: vector containing the values of the joints 1, 2 and 3. 0006 % T: orientation of the last reference system. 0007 % wrist: select -1 or 1 for two possible solutions (wrist up, wrist down) 0008 % 0009 % See also DIRECTKINEMATIC. 0010 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0011 0012 % Copyright (C) 2012, by Arturo Gil Aparicio 0013 % 0014 % This file is part of ARTE (A Robotics Toolbox for Education). 0015 % 0016 % ARTE is free software: you can redistribute it and/or modify 0017 % it under the terms of the GNU Lesser General Public License as published by 0018 % the Free Software Foundation, either version 3 of the License, or 0019 % (at your option) any later version. 0020 % 0021 % ARTE is distributed in the hope that it will be useful, 0022 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0023 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0024 % GNU Lesser General Public License for more details. 0025 % 0026 % You should have received a copy of the GNU Leser General Public License 0027 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0028 0029 function q = solve_spherical_wrist(robot, q, T, wrist, method) 0030 0031 switch method 0032 0033 %algebraic solution 0034 case 'algebraic' 0035 T01=dh(robot, q, 1); 0036 T12=dh(robot, q, 2); 0037 T23=dh(robot, q, 3); 0038 0039 Q=inv(T23)*inv(T12)*inv(T01)*T; 0040 0041 %detect the degenerate case when q(5)=0, this leads to zeros 0042 % in Q13, Q23, Q31 and Q32 and Q33=1 0043 thresh=1e-12; 0044 %detect if q(5)==0 0045 % this happens when cos(q5) in the matrix Q is close to 1 0046 if abs(Q(3,3)-1)>thresh 0047 %normal solution 0048 if wrist==1 %wrist up 0049 q(4)=atan2(-Q(2,3),-Q(1,3)); 0050 q(6)=atan2(-Q(3,2),Q(3,1)); 0051 %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3)); 0052 else %wrist down 0053 q(4)=atan2(-Q(2,3),-Q(1,3))-pi; 0054 q(6)=atan2(-Q(3,2),Q(3,1))+pi; 0055 %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3)); 0056 end 0057 if abs(cos(q(6)+q(4)))>thresh 0058 cq5=(Q(1,1)+Q(2,2))/cos(q(4)+q(6))-1; 0059 end 0060 if abs(sin(q(6)+q(4)))>thresh 0061 cq5=(-Q(1,2)+Q(2,1))/sin(q(4)+q(6))-1; 0062 end 0063 if abs(sin(q(6)))>thresh 0064 sq5=-Q(3,2)/sin(q(6)); 0065 end 0066 if abs(cos(q(6)))>thresh 0067 sq5=Q(3,1)/cos(q(6)); 0068 end 0069 q(5)=atan2(sq5,cq5); 0070 0071 else %degenerate solution, in this case, q4 cannot be determined, 0072 % so q(4)=0 is assigned 0073 if wrist==1 %wrist up 0074 q(4)=0; 0075 q(5)=0; 0076 q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2)); 0077 else %wrist down 0078 q(4)=-pi; 0079 q(5)=0; 0080 q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))+pi; 0081 end 0082 0083 end 0084 0085 %geometric solution 0086 case 'geometric' 0087 % T is the noa matrix defining the position/orientation of the end 0088 % effector's reference system 0089 vx6=T(1:3,1); 0090 vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5 0091 0092 % Obtain the position and orientation of the system 3 0093 % using the already computed joints q1, q2 and q3 0094 T01=dh(robot, q, 1); 0095 T12=dh(robot, q, 2); 0096 T23=dh(robot, q, 3); 0097 T03=T01*T12*T23; 0098 0099 vx3=T03(1:3,1); 0100 vy3=T03(1:3,2); 0101 vz3=T03(1:3,3); 0102 0103 % find z4 normal to the plane formed by z3 and a 0104 vz4=cross(vz3, vz5); % end effector's vector a: T(1:3,3) 0105 0106 % in case of degenerate solution, 0107 % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution 0108 if norm(vz4) <= 0.000001 0109 if wrist == 1 %wrist up 0110 q(4)=0; 0111 else 0112 q(4)=-pi; %wrist down 0113 end 0114 else 0115 %this is the normal and most frequent solution 0116 cosq4=wrist*dot(-vy3,vz4); 0117 sinq4=wrist*dot(vx3,vz4); 0118 q(4)=atan2(sinq4, cosq4); 0119 end 0120 %propagate the value of q(4) to compute the system 4 0121 T34=dh(robot, q, 4); 0122 T04=T03*T34; 0123 vx4=T04(1:3,1); 0124 vy4=T04(1:3,2); 0125 0126 % solve for q5 0127 cosq5=dot(vy4,vz5); 0128 sinq5=dot(-vx4,vz5); 0129 q(5)=atan2(sinq5, cosq5); 0130 0131 %propagate now q(5) to compute T05 0132 T45=dh(robot, q, 5); 0133 T05=T04*T45; 0134 vx5=T05(1:3,1); 0135 vy5=T05(1:3,2); 0136 0137 % solve for q6 0138 cosq6=dot(vx6,vx5); 0139 sinq6=dot(vx6,vy5); 0140 q(6)=atan2(sinq6, cosq6); 0141 0142 0143 0144 otherwise 0145 disp('no method specified in solve_spherical_wrist'); 0146 end