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.
281 lines
6.3 KiB
281 lines
6.3 KiB
% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
|
|
%
|
|
% Copyright (C) 1993-2011, by Peter I. Corke
|
|
%
|
|
% This file is part of The Robotics Toolbox for MATLAB (RTB).
|
|
%
|
|
% RTB 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.
|
|
%
|
|
% RTB 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 Lesser General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Leser General Public License
|
|
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
|
|
%
|
|
% http://www.petercorke.com
|
|
function v = subsref(l, s)
|
|
switch s(1).type
|
|
case '.'
|
|
% NOTE WELL: the following code can't use getfield() since
|
|
% getfield() uses this, and Matlab will crash!!
|
|
|
|
el = char(s(1).subs);
|
|
switch el,
|
|
case 'alpha',
|
|
v = l.alpha;
|
|
case 'a',
|
|
v = l.a;
|
|
case 'theta',
|
|
v = l.theta;
|
|
case 'd',
|
|
v = l.d;
|
|
case 'offset',
|
|
v = l.offset;
|
|
case 'sigma',
|
|
v = l.sigma;
|
|
case 'RP',
|
|
if l.sigma == 0,
|
|
v = 'R';
|
|
else
|
|
v = 'P';
|
|
end
|
|
case 'mdh',
|
|
v = l.mdh;
|
|
case 'G',
|
|
v = l.G;
|
|
case 'I',
|
|
v = l.I;
|
|
case 'r',
|
|
v = l.r;
|
|
case 'Jm',
|
|
v = l.Jm;
|
|
case 'B',
|
|
v = l.B;
|
|
case 'Tc',
|
|
v = l.Tc;
|
|
case 'qlim',
|
|
v = l.qlim;
|
|
case 'islimit',
|
|
if s(2).type ~= '()'
|
|
error('expecting argument for islimit method');
|
|
end
|
|
q = s(2).subs{1};
|
|
v = (q > l.qlim(2)) - (q < l.qlim(1));
|
|
case 'm',
|
|
v = l.m;
|
|
case 'dh',
|
|
v = [l.alpha l.A l.theta l.D l.sigma];
|
|
case 'dyn',
|
|
dyn(l);
|
|
case 'A',
|
|
if s(2).type ~= '()'
|
|
error('expecting argument for A method');
|
|
else
|
|
|
|
args = s(2).subs;
|
|
v = A(l,args{1});
|
|
end
|
|
|
|
case 'nofriction',
|
|
q = s(2).subs;
|
|
v = nofriction(l,q{:});
|
|
otherwise,
|
|
disp('Unknown method ref')
|
|
end
|
|
case '()'
|
|
|
|
if numel(s) == 1
|
|
|
|
v = builtin('subsref', l, s);
|
|
else
|
|
z = s(1).subs; % cell array
|
|
|
|
k = z{1};
|
|
l = l(k);
|
|
v_subset = subsref(l, s(2:end));
|
|
|
|
v = v_subset; % put subset back;
|
|
end
|
|
otherwise
|
|
error('only .field supported')
|
|
end %switch
|
|
|
|
|
|
endfunction
|
|
|
|
function T = A(L, q)
|
|
%Link.A Link transform matrix
|
|
%
|
|
% T = L.A(Q) is the 4x4 link homogeneous transformation matrix corresponding
|
|
% to the link variable Q which is either theta (revolute) or d (prismatic).
|
|
%
|
|
% Notes::
|
|
% - For a revolute joint the theta parameter of the link is ignored, and Q used instead.
|
|
% - For a prismatic joint the d parameter of the link is ignored, and Q used instead.
|
|
% - The link offset parameter is added to Q before computation of the transformation matrix.
|
|
if L.mdh == 0
|
|
T = linktran([L.alpha L.a L.theta L.d L.sigma], ...
|
|
q+L.offset);
|
|
else
|
|
T = mlinktran([L.alpha L.a L.theta L.d L.sigma], ...
|
|
q+L.offset);
|
|
end
|
|
endfunction % A()
|
|
|
|
function t = linktran(a, b, c, d)
|
|
%LINKTRAN Compute the link transform from kinematic parameters
|
|
%
|
|
% LINKTRAN(alpha, an, theta, dn)
|
|
% LINKTRAN(DH, q) is a homogeneous
|
|
% transformation between link coordinate frames.
|
|
%
|
|
% alpha is the link twist angle
|
|
% an is the link length
|
|
% theta is the link rotation angle
|
|
% dn is the link offset
|
|
% sigma is 0 for a revolute joint, non-zero for prismatic
|
|
%
|
|
% In the second case, q is substitued for theta or dn according to sigma.
|
|
%
|
|
% Based on the standard Denavit and Hartenberg notation.
|
|
|
|
% Copright (C) Peter Corke 1993
|
|
|
|
if nargin == 4,
|
|
alpha = a;
|
|
an = b;
|
|
theta = c;
|
|
dn = d;
|
|
else
|
|
if numcols(a) < 4,
|
|
error('too few columns in DH matrix');
|
|
end
|
|
alpha = a(1);
|
|
an = a(2);
|
|
if numcols(a) > 4,
|
|
if a(5) == 0, % revolute
|
|
theta = b;
|
|
dn = a(4);
|
|
else % prismatic
|
|
theta = a(3);
|
|
dn = b;
|
|
end
|
|
else
|
|
theta = b; % assume revolute if sigma not given
|
|
dn = a(4);
|
|
end
|
|
end
|
|
sa = sin(alpha); ca = cos(alpha);
|
|
st = sin(theta); ct = cos(theta);
|
|
|
|
t = [ ct -st*ca st*sa an*ct
|
|
st ct*ca -ct*sa an*st
|
|
0 sa ca dn
|
|
0 0 0 1];
|
|
endfunction
|
|
%MLINKTRANS Compute the link transform from kinematic parameters
|
|
%
|
|
% MLINKTRANS(alpha, an, theta, dn)
|
|
% MLINKTRANS(DH, q) is a homogeneous
|
|
% transformation between link coordinate frames.
|
|
%
|
|
% alpha is the link twist angle
|
|
% an is the link length
|
|
% theta is the link rotation angle
|
|
% dn is the link offset
|
|
% sigma is 0 for a revolute joint, non-zero for prismatic
|
|
%
|
|
% In the second case, q is substitued for theta or dn according to sigma.
|
|
%
|
|
% Based on the modified Denavit and Hartenberg notation.
|
|
|
|
% Copright (C) Peter Corke 1993
|
|
function t = mlinktrans(a, b, c, d)
|
|
|
|
if nargin == 4,
|
|
alpha = a;
|
|
an = b;
|
|
theta = c;
|
|
dn = d;
|
|
else
|
|
if numcols(a) < 4,
|
|
error('too few columns in DH matrix');
|
|
end
|
|
alpha = a(1);
|
|
an = a(2);
|
|
if numcols(a) > 4,
|
|
if a(5) == 0, % revolute
|
|
theta = b;
|
|
dn = a(4);
|
|
else % prismatic
|
|
theta = a(3);
|
|
dn = b;
|
|
end
|
|
else
|
|
theta = b; % assume revolute if no sigma given
|
|
dn = a(4);
|
|
end
|
|
end
|
|
sa = sin(alpha); ca = cos(alpha);
|
|
st = sin(theta); ct = cos(theta);
|
|
|
|
t = [ ct -st 0 an
|
|
st*ca ct*ca -sa -sa*dn
|
|
st*sa ct*sa ca ca*dn
|
|
0 0 0 1];
|
|
endfunction
|
|
|
|
function dyn(l)
|
|
%Link.dyn Display the inertial properties of link
|
|
%
|
|
% L.dyn() displays the inertial properties of the link object in a multi-line format.
|
|
% The properties shown are mass, centre of mass, inertia, friction, gear ratio
|
|
% and motor properties.
|
|
%
|
|
% If L is a vector of Link objects show properties for each element.
|
|
|
|
if length(l) > 1
|
|
for j=1:length(l)
|
|
ll = l(j);
|
|
fprintf('%d: %f; %f %f %f; %f %f %f\n', ...
|
|
j, ll.m, ll.r, diag(ll.I));
|
|
%dyn(ll);
|
|
end
|
|
return;
|
|
end
|
|
|
|
display(l);
|
|
if ~isempty(l.m)
|
|
fprintf(' m = %f\n', l.m)
|
|
end
|
|
if ~isempty(l.r)
|
|
fprintf(' r = %f %f %f\n', l.r);
|
|
end
|
|
if ~isempty(l.I)
|
|
fprintf(' I = | %f %f %f |\n', l.I(1,:));
|
|
fprintf(' | %f %f %f |\n', l.I(2,:));
|
|
fprintf(' | %f %f %f |\n', l.I(3,:));
|
|
end
|
|
if ~isempty(l.Jm)
|
|
fprintf(' Jm = %f\n', l.Jm);
|
|
end
|
|
if ~isempty(l.B)
|
|
fprintf(' Bm = %f\n', l.B);
|
|
end
|
|
if ~isempty(l.Tc)
|
|
fprintf(' Tc = %f(+) %f(-)\n', l.Tc(1), l.Tc(2));
|
|
end
|
|
if ~isempty(l.G)
|
|
fprintf(' G = %f\n', l.G);
|
|
end
|
|
if ~isempty(l.qlim)
|
|
fprintf(' qlim = %f to %f\n', l.qlim(1), l.qlim(2));
|
|
end
|
|
endfunction % dyn()
|
|
|
|
|