%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMPUTE_END_VELOCITY computes the velocity V of the end effector as a function of the joint coordinates and joint speeds. If robot.J is defined the Jacobian is evaluated to compute a given velocity. The result depends on the definition of J On the other hand, if robot.J is an empty array a conventional Jacobian is computed instead. The result V is then V = [vn wn]' where vn is the linear speed of the end effector and wn is the angular speed, both expressed in the base coordinate system of the robot. See also COMPUTE_JACOBIAN. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 26/04/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % COMPUTE_END_VELOCITY computes the velocity V of the end effector as a 0003 % function of the joint coordinates and joint speeds. 0004 % If robot.J is defined the Jacobian is evaluated to compute a given 0005 % velocity. The result depends on the definition of J 0006 % On the other hand, if robot.J is an empty array a conventional Jacobian 0007 % is computed instead. The result V is then V = [vn wn]' where vn is the 0008 % linear speed of the end effector and wn is the angular speed, both 0009 % expressed in the base coordinate system of the robot. 0010 % 0011 % See also COMPUTE_JACOBIAN. 0012 % 0013 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0014 % email: arturo.gil@umh.es date: 26/04/2012 0015 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0016 0017 % Copyright (C) 2012, by Arturo Gil Aparicio 0018 % 0019 % This file is part of ARTE (A Robotics Toolbox for Education). 0020 % 0021 % ARTE is free software: you can redistribute it and/or modify 0022 % it under the terms of the GNU Lesser General Public License as published by 0023 % the Free Software Foundation, either version 3 of the License, or 0024 % (at your option) any later version. 0025 % 0026 % ARTE is distributed in the hope that it will be useful, 0027 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0028 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0029 % GNU Lesser General Public License for more details. 0030 % 0031 % You should have received a copy of the GNU Leser General Public License 0032 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0033 function V = compute_end_velocity(robot, q, qd) 0034 0035 theta = eval(robot.DH.theta); 0036 d = eval(robot.DH.d); 0037 a = eval(robot.DH.a); 0038 alfa = eval(robot.DH.alpha); 0039 0040 0041 if ~isempty(robot.J) 0042 if robot.debug 0043 fprintf('\n:compute_end_velocity: Computing end effector speed %s robot with %d DOFs', robot.name, n); 0044 fprintf('\nUsing user defined J: %s', robot.J); 0045 end 0046 0047 J = eval(robot.J); 0048 0049 n=length(theta); %# number of DOFs 0050 0051 V=J*qd(1:3)'; 0052 else%computing J from D-H parameters 0053 if robot.debug 0054 disp('\n:compute_end_velocity: Computing conventional Jacobian'); 0055 end 0056 J = compute_jacobian(robot, q, qd); 0057 %now V = [vn wn]'; 0058 V = J*qd'; 0059 end