solve_spherical_wrist2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   q = solve_spherical_wrist2(robot, q, T, wrist)
   Solves the inverse kinematic problem for a spherical wrist. This is for
   the particular RELATIVE orientation of the last three reference systems in
   ABB robots

   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_wrist2(robot, q, T, wrist)
0003 %   Solves the inverse kinematic problem for a spherical wrist. This is for
0004 %   the particular RELATIVE orientation of the last three reference systems in
0005 %   ABB robots
0006 %
0007 %   robot: robot structure.
0008 %   q: vector containing the values of the joints 1, 2 and 3.
0009 %   T: orientation of the last reference system.
0010 %   wrist: select -1 or 1 for two possible solutions (wrist up, wrist down)
0011 %
0012 %    See also DIRECTKINEMATIC.
0013 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0014 
0015 % Copyright (C) 2012, by Arturo Gil Aparicio
0016 %
0017 % This file is part of ARTE (A Robotics Toolbox for Education).
0018 %
0019 % ARTE is free software: you can redistribute it and/or modify
0020 % it under the terms of the GNU Lesser General Public License as published by
0021 % the Free Software Foundation, either version 3 of the License, or
0022 % (at your option) any later version.
0023 %
0024 % ARTE is distributed in the hope that it will be useful,
0025 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0026 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0027 % GNU Lesser General Public License for more details.
0028 %
0029 % You should have received a copy of the GNU Leser General Public License
0030 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0031 
0032 function q = solve_spherical_wrist2(robot, q, T, wrist, method)
0033 
0034 switch method
0035     
0036     %algebraic solution
0037     case 'algebraic'
0038         A01=dh(robot, q, 1);
0039         A12=dh(robot, q, 2);
0040         A23=dh(robot, q, 3);
0041         
0042         Q=inv(A23)*inv(A12)*inv(A01)*T;
0043         
0044         %detect the degenerate case when q(5)=0, this leads to zeros
0045         % in Q13, Q23, Q31 and Q32 and Q33=1
0046         thresh=1e-12;
0047         %detect if q(5)==0
0048         % this happens when cos(q5) in the matrix Q is close to 1
0049         if abs(Q(3,3)-1)>thresh
0050             %normal solution
0051             if wrist==1 %wrist up
0052                 q(4)=atan2(Q(2,3),Q(1,3));
0053                 q(6)=atan2(-Q(3,2),Q(3,1));
0054             else %wrist down
0055                 q(4)=atan2(Q(2,3),Q(1,3))+pi;
0056                 q(6)=atan2(-Q(3,2),Q(3,1))+pi;
0057             end
0058             if abs(cos(q(6)+q(4)))>thresh
0059                 cq5=(-Q(1,1)-Q(2,2))/cos(q(4)+q(6))-1;
0060             end
0061             if abs(sin(q(6)+q(4)))>thresh
0062                 cq5=(Q(1,2)-Q(2,1))/sin(q(4)+q(6))-1;
0063             end
0064             if abs(sin(q(6)))>thresh
0065                 sq5=Q(3,2)/sin(q(6));
0066             end
0067             if abs(cos(q(6)))>thresh
0068                 sq5=-Q(3,1)/cos(q(6));
0069             end
0070             q(5)=atan2(sq5,cq5);
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         %algebraic solution
0086     case 'geometric'
0087         
0088         % Obtain the position and orientation of the system 3
0089         % using the already computed joints q1, q2 and q3
0090         T01=dh(robot, q, 1);
0091         T12=dh(robot, q, 2);
0092         T23=dh(robot, q, 3);
0093         T03=T01*T12*T23;
0094         
0095         x3=T03(1:3,1);
0096         y3=T03(1:3,2);
0097         z3=T03(1:3,3);
0098         
0099         % T= [ nx ox ax Px;
0100         %     ny oy ay Py;
0101         %     nz oz az Pz];
0102         a=T(1:3,3);
0103         
0104         % find z4 normal to the plane formed by z3 and a
0105         z4=cross(z3, a);    % end effector's vector a: T(1:3,3)
0106         
0107         % in case of degenerate solution,
0108         % when z3 and z6 are parallel, choose q(4)=0 as solution
0109         if norm(z4) <= 0.000001
0110             if wrist == 1 %wrist up
0111                 q(4)=0;
0112             else
0113                 q(4)=-pi;
0114             end
0115         else
0116             cq4=wrist*dot(z4, -y3);
0117             sq4=wrist*dot(z4, x3);
0118             q(4)=atan2(sq4, cq4);
0119         end
0120         
0121         % solve for q5
0122         T34=dh(robot, q, 4);
0123         T04=T03*T34;
0124         x4=T04(1:3, 1);
0125         y4=T04(1:3, 2);
0126         
0127         z5=T(1:3, 3); % The vector a T(1:3,3) is coincident with z5
0128         
0129         cq5=dot(z5, y4);
0130         sq5=dot(z5, -x4);
0131         q(5)=atan2(sq5, cq5);
0132         
0133         % solve for q6
0134         x6=T(1:3, 1);
0135         
0136         T45=dh(robot, q, 5);
0137         T05=T04*T45;
0138         x5=T05(1:3, 1);
0139         y5=T05(1:3, 2);
0140         
0141         cq6=dot(x6, -x5);
0142         sq6=dot(x6, -y5);
0143         q(6)=atan2(sq6, cq6);
0144         
0145         
0146     otherwise
0147         disp('no method specified in solve_spherical_wrist');
0148 end