%SerialLink.GRAVJAC Fast gravity load and Jacobian % % [TAU,JAC0] = R.gravjac(Q) is the generalised joint force/torques due to % gravity (1xN) and the manipulator Jacobian in the base frame (6xN) for % robot pose Q (1xN), where N is the number of robot joints. % % [TAU,JAC0] = R.gravjac(Q,GRAV) as above but gravity is given explicitly % by GRAV (3x1). % % Trajectory operation:: % % If Q is MxN where N is the number of robot joints then a trajectory is % assumed where each row of Q corresponds to a pose. TAU (MxN) is the % generalised joint torque, each row corresponding to an input pose, and % JAC0 (6xNxM) where each plane is a Jacobian corresponding to an input pose. % % Notes:: % - The gravity vector is defined by the SerialLink property if not explicitly given. % - Does not use inverse dynamics function RNE. % - Faster than computing gravity and Jacobian separately. % % Author:: % Bryan Moutrie % % See also SerialLink.pay, SerialLink, SerialLink.gravload, SerialLink.jacob0. % Copyright (C) Bryan Moutrie, 2013-2015 % Licensed under the GNU Lesser General Public License % see full file for full statement % % LICENSE STATEMENT: % % This file is part of pHRIWARE. % % pHRIWARE is free software: you can redistribute it and/or modify % it under the terms of the GNU Lesser General Public License as % published by the Free Software Foundation, either version 3 of % the License, or (at your option) any later version. % % pHRIWARE is distributed in the hope that it will be useful, % but WITHOUT ANY WARRANTY; without even the implied warranty of % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % GNU General Public License for more details. % % You should have received a copy of the GNU Lesser General Public % License along with pHRIWARE. If not, see . % function [tauB, J] = gravjac(robot, q, grav) n = robot.n; revolute = [robot.links(:).isrevolute]; if ~robot.mdh baseAxis = robot.base(1:3,3); baseOrigin = robot.base(1:3,4); end poses = size(q, 1); tauB = zeros(poses, n); if nargout == 2, J = zeros(6, robot.n, poses); end % Forces force = zeros(3,n); if nargin < 3, grav = robot.gravity; end for joint = 1: n force(:,joint) = robot.links(joint).m * grav; end % Centre of masses (local frames) r = zeros(4,n); for joint = 1: n r(:,joint) = [robot.links(joint).r'; 1]; end com_arr = zeros(3, n); for pose = 1: poses [Te, T] = robot.fkine(q(pose,:)); jointOrigins = squeeze(T(1:3,4,:)); jointAxes = squeeze(T(1:3,3,:)); if ~robot.mdh jointOrigins = [baseOrigin, jointOrigins(:,1:end-1)]; jointAxes = [baseAxis, jointAxes(:,1:end-1)]; end % Backwards recursion for joint = n: -1: 1 com = T(:,:,joint) * r(:,joint); % C.o.M. in world frame, homog com_arr(:,joint) = com(1:3); % Add it to the distal others t = 0; for link = joint: n % for all links distal to it if revolute(joint) d = com_arr(:,link) - jointOrigins(:,joint); t = t + cross3(d, force(:,link)); % Though r x F would give the applied torque and not the % reaction torque, the gravity vector is nominally in the % positive z direction, not negative, hence the force is % the reaction force else t = t + force(:,link); %force on prismatic joint end end tauB(pose,joint) = t' * jointAxes(:,joint); end if nargout == 2 J(:,:,pose) = makeJ(jointOrigins,jointAxes,Te(1:3,4),revolute); end end end function J = makeJ(O,A,e,r) J(4:6,:) = A; for j = 1:length(r) if r(j) J(1:3,j) = cross3(A(:,j),e-O(:,j)); else J(:,j) = J([4 5 6 1 2 3],j); %J(1:3,:) = 0; end end end function c = cross3(a,b) c(3,1) = a(1)*b(2) - a(2)*b(1); c(1,1) = a(2)*b(3) - a(3)*b(2); c(2,1) = a(3)*b(1) - a(1)*b(3); end