You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
81 lines
2.5 KiB
81 lines
2.5 KiB
%SerialLink.PAYCAP Static payload capacity of a robot
|
|
%
|
|
% [WMAX,J] = R.paycap(Q, W, F, TLIM) returns the maximum permissible
|
|
% payload wrench WMAX (1x6) applied at the end-effector, and the index of
|
|
% the joint J which hits its force/torque limit at that wrench. Q (1xN) is
|
|
% the manipulator pose, W the payload wrench (1x6), F the wrench reference
|
|
% frame (either '0' or 'n') and TLIM (2xN) is a matrix of joint
|
|
% forces/torques (first row is maximum, second row minimum).
|
|
%
|
|
% Trajectory operation::
|
|
%
|
|
% In the case Q is MxN then WMAX is Mx6 and J is Mx1 where the rows are the
|
|
% results at the pose given by corresponding row of Q.
|
|
%
|
|
% Notes::
|
|
% - Wrench vector and Jacobian must be from the same reference frame
|
|
% - Tool transforms are taken into consideration for F = 'n'.
|
|
%
|
|
% Author::
|
|
% Bryan Moutrie
|
|
%
|
|
% See also SerialLink.pay, SerialLink.gravjac, SerialLink.gravload.
|
|
|
|
% 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 <http://www.gnu.org/licenses/>.
|
|
|
|
function [wM, j] = paycap(robot, q, w, f, tauR)
|
|
|
|
if robot.fast
|
|
tauB = robot.gravload(q);
|
|
tauP = robot.rne(q, zeros(size(q)), zeros(size(q)), [0; 0; 0], unit(w));
|
|
elseif f == '0'
|
|
[tauB, J] = gravjac(robot, q);
|
|
tauP = robot.pay(unit(w), J);
|
|
elseif f == 'n'
|
|
tauB = gravjac(robot, q);
|
|
tauP = robot.pay(unit(w), q, 'n');
|
|
end
|
|
|
|
M = tauP > 0;
|
|
m = ~M;
|
|
|
|
TAUm = ones(size(tauB));
|
|
TAUM = ones(size(tauB));
|
|
for c = 1:robot.n
|
|
TAUM(:,c) = tauR(1,c);
|
|
TAUm(:,c) = tauR(2,c);
|
|
end
|
|
|
|
WM = zeros(size(tauB));
|
|
WM(M) = (TAUM(M) - tauB(M)) ./ tauP(M);
|
|
WM(m) = (TAUm(m) - tauB(m)) ./ tauP(m);
|
|
|
|
WM(isinf(WM)) = Inf; % Makes -Inf values Inf
|
|
|
|
[wM, j] = min(WM,[],2);
|
|
|
|
end
|
|
|
|
function u = unit(v)
|
|
u = v/norm(v);
|
|
end
|
|
|