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