inversekinematic_rv_6s

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   Q = INVERSEKINEMATIC_RV_6S_6DOF(robot, T)    
   Solves the inverse kinematic problem for the MITSUBISHI RV-6S 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_RV_6S_6DOF returns 8 possible solutions, thus,
   Q is a 6x8 matrix where each column stores 6 feasible joint values.

   
   Example code:

   robot=load_robot('mitsubishi', 'rv-6s');
   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_RV_6S_6DOF(robot, T)
0003 %   Solves the inverse kinematic problem for the MITSUBISHI RV-6S 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_RV_6S_6DOF 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('mitsubishi', 'rv-6s');
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_rv_6s(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 L6=d(6);
0054 
0055 
0056 %T= [ nx ox ax Px;
0057 %     ny oy ay Py;
0058 %     nz oz az Pz];
0059 Px=T(1,4);
0060 Py=T(2,4);
0061 Pz=T(3,4);
0062 
0063 %Compute the position of the wrist, being W the Z component of the end effector's system
0064 W = T(1:3,3);
0065 
0066 % Pm: wrist position
0067 Pm = [Px Py Pz]' - L6*W; 
0068 
0069 %first joint, two possible solutions admited:
0070 % if q(1) is a solution, then q(1) + pi is also a solution
0071 q1=atan2(Pm(2), Pm(1));
0072 
0073 
0074 %solve for q2
0075 q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
0076 %the other possible solution is q1 + pi
0077 q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);
0078 
0079 %solve for q3
0080 q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
0081 %solver for q3 for both cases
0082 q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);
0083 
0084 %the next matrix doubles each column. For each two columns, two different
0085 %configurations for theta4, theta5 and theta6 will be computed. These
0086 %configurations are generally referred as wrist up and wrist down solution
0087 q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;   
0088      q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
0089      q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
0090      0          0          0         0         0      0       0       0;
0091      0          0          0         0         0      0       0       0;
0092      0          0          0         0         0      0       0       0];
0093 
0094 
0095 %leave only the real part of the solutions
0096 q=real(q);
0097 
0098 %Note that in this robot, the joint q3 has a non-simmetrical range.
0099 
0100 
0101 %normalize q to [-pi, pi]
0102 q(1,:) = normalize(q(1,:));
0103 q(2,:) = normalize(q(2,:));
0104 
0105 % solve for the last three joints
0106 % for any of the possible combinations (theta1, theta2, theta3)
0107 for i=1:2:size(q,2),
0108     qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
0109     qtemp(4:6)=normalize(qtemp(4:6));
0110     q(:,i)=qtemp;
0111     
0112     qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
0113     qtemp(4:6)=normalize(qtemp(4:6));
0114     q(:,i+1)=qtemp;
0115 end
0116 
0117 
0118 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0119 % solve for second joint theta2, two different
0120 % solutions are returned, corresponding
0121 % to elbow up and down solution
0122 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0123 function q2 = solve_for_theta2(robot, q, Pm)
0124 
0125 %Evaluate the parameters
0126 theta = eval(robot.DH.theta);
0127 d = eval(robot.DH.d);
0128 a = eval(robot.DH.a);
0129 alpha = eval(robot.DH.alpha);
0130 
0131 %See geometry
0132 L2=a(2);
0133 L3=d(4);
0134 A2=a(3);
0135 
0136 %See geometry of the robot
0137 %compute L4
0138 L4 = sqrt(A2^2 + L3^2);
0139 
0140 %The inverse kinematic problem can be solved as in the IRB 140 (for example)
0141 
0142 %given q1 is known, compute first DH transformation
0143 T01=dh(robot, q, 1);
0144 
0145 %Express Pm in the reference system 1, for convenience
0146 p1 = inv(T01)*[Pm; 1];
0147 
0148 r = sqrt(p1(1)^2 + p1(2)^2);
0149 
0150 beta = atan2(-p1(2), p1(1));
0151 gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));
0152 
0153 %return two possible solutions
0154 %elbow up and elbow down
0155 %the order here is important and is coordinated with the function
0156 %solve_for_theta3
0157 q2(1) = pi/2 - beta - gamma; %elbow up
0158 q2(2) = pi/2 - beta + gamma; %elbow down
0159 
0160 
0161 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0162 % solve for third joint theta3, two different
0163 % solutions are returned, corresponding
0164 % to elbow up and down solution
0165 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0166 function q3 = solve_for_theta3(robot, q, Pm)
0167 
0168 %Evaluate the parameters
0169 theta = eval(robot.DH.theta);
0170 d = eval(robot.DH.d);
0171 a = eval(robot.DH.a);
0172 alpha = eval(robot.DH.alpha);
0173 
0174 %See geometry
0175 L2=a(2);
0176 L3=d(4);
0177 A2=a(3);
0178 
0179 %See geometry of the robot
0180 %compute L4
0181 L4 = sqrt(A2^2 + L3^2);
0182 
0183 %the angle phi is fixed
0184 phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));
0185 
0186 %given q1 is known, compute first DH transformation
0187 T01=dh(robot, q, 1);
0188 
0189 %Express Pm in the reference system 1, for convenience
0190 p1 = inv(T01)*[Pm; 1];
0191 
0192 r = sqrt(p1(1)^2 + p1(2)^2);
0193 
0194 eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));
0195 
0196 %return two possible solutions
0197 %elbow up and elbow down solutions
0198 %the order here is important
0199 q3(1) = pi - phi- eta; 
0200 q3(2) = pi - phi + eta; 
0201 
0202