%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMPUTE_JACOBIAN computes the conventional jacobian as a function of the joint coordinates. The algorithm is written as is for educational purposes and translated from: Robot Analysis. The mechanics of Serial and parallel manipulators. Lung-Wen Tsai. John Wiley and Sons. ISBN: 0-471-32593-7. Page 186. Given the conventional Jacobian computed in this way. The end effectors velocity defined as V = [vn wn]' can be computed as V = J*qd, where vn and wn are the end effector's linear and angular speed respectively and qd is the joint speed. See also DIRECTKINEMATIC. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 26/04/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % COMPUTE_JACOBIAN computes the conventional jacobian as a function 0003 % of the joint coordinates. The algorithm is written as is for educational 0004 % purposes and translated from: 0005 % 0006 % Robot Analysis. The mechanics of Serial and parallel manipulators. 0007 % Lung-Wen Tsai. John Wiley and Sons. ISBN: 0-471-32593-7. Page 186. 0008 % 0009 % Given the conventional Jacobian computed in this way. The end effectors 0010 % velocity defined as V = [vn wn]' can be computed as V = J*qd, where vn 0011 % and wn are the end effector's linear and angular speed respectively and 0012 % qd is the joint speed. 0013 % 0014 % See also DIRECTKINEMATIC. 0015 % 0016 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0017 % email: arturo.gil@umh.es date: 26/04/2012 0018 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0019 0020 % Copyright (C) 2012, by Arturo Gil Aparicio 0021 % 0022 % This file is part of ARTE (A Robotics Toolbox for Education). 0023 % 0024 % ARTE is free software: you can redistribute it and/or modify 0025 % it under the terms of the GNU Lesser General Public License as published by 0026 % the Free Software Foundation, either version 3 of the License, or 0027 % (at your option) any later version. 0028 % 0029 % ARTE is distributed in the hope that it will be useful, 0030 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0031 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0032 % GNU Lesser General Public License for more details. 0033 % 0034 % You should have received a copy of the GNU Leser General Public License 0035 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0036 function J = compute_jacobian(robot, q) 0037 0038 theta = eval(robot.DH.theta); 0039 d = eval(robot.DH.d); 0040 a = eval(robot.DH.a); 0041 alfa = eval(robot.DH.alpha); 0042 0043 %number of DOF 0044 n = length(theta); 0045 0046 %base rotation 0047 R0 = eye(3); 0048 0049 z = [0 0 1]'; 0050 0051 %compute zi vectors 0052 for i=1:n-1, 0053 DH = dh(robot, q, i); 0054 0055 R0=R0*DH(1:3,1:3); 0056 r=DH(1:3,4); 0057 0058 zi = R0*[0 0 1]'; 0059 z = [z zi]; 0060 end 0061 0062 %compute pin vectors that connect the ith reference system with the end 0063 %effector for i=0: n 0064 T = directkinematic(robot, q); 0065 p =[T(1:3,4)]; 0066 for i=1:n-1, 0067 DH = dh(robot, q, i); 0068 T = inv(DH)*T; 0069 p = [p T(1:3,4)]; 0070 end 0071 0072 %now compute conventional Jacobian 0073 J= []; 0074 for i=1:n, 0075 %rotational 0076 if robot.kind(i)=='R' 0077 Ji =[cross(z(:,i),p(:,i));z(:,i) ]; 0078 else %translational joint 0079 Ji =[z(:,i); 0]; 0080 end 0081 J = [J Ji]; 0082 end