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.
810 lines
30 KiB
810 lines
30 KiB
%LINK Robot manipulator Link class
|
|
%
|
|
% A Link object holds all information related to a robot link such as
|
|
% kinematics parameters, rigid-body inertial parameters, motor and
|
|
% transmission parameters.
|
|
%
|
|
% Methods::
|
|
% A link transform matrix
|
|
% RP joint type: 'R' or 'P'
|
|
% friction friction force
|
|
% nofriction Link object with friction parameters set to zero
|
|
% dyn display link dynamic parameters
|
|
% islimit test if joint exceeds soft limit
|
|
% isrevolute test if joint is revolute
|
|
% isprismatic test if joint is prismatic
|
|
% display print the link parameters in human readable form
|
|
% char convert to string
|
|
%
|
|
% Properties (read/write)::
|
|
%
|
|
% theta kinematic: joint angle
|
|
% d kinematic: link offset
|
|
% a kinematic: link length
|
|
% alpha kinematic: link twist
|
|
% sigma kinematic: 0 if revolute, 1 if prismatic
|
|
% mdh kinematic: 0 if standard D&H, else 1
|
|
% offset kinematic: joint variable offset
|
|
% qlim kinematic: joint variable limits [min max]
|
|
%
|
|
% m dynamic: link mass
|
|
% r dynamic: link COG wrt link coordinate frame 3x1
|
|
% I dynamic: link inertia matrix, symmetric 3x3, about link COG.
|
|
% B dynamic: link viscous friction (motor referred)
|
|
% Tc dynamic: link Coulomb friction
|
|
%
|
|
% G actuator: gear ratio
|
|
% Jm actuator: motor inertia (motor referred)
|
|
%
|
|
% Notes::
|
|
% - This is reference class object
|
|
% - Link objects can be used in vectors and arrays
|
|
%
|
|
% References::
|
|
% - Robotics, Vision & Control, Chap 7
|
|
% P. Corke, Springer 2011.
|
|
%
|
|
% See also Link, Revolute, Prismatic, SerialLink.
|
|
|
|
% 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/>.
|
|
|
|
classdef Link < handle
|
|
properties
|
|
% kinematic parameters
|
|
theta % kinematic: link angle
|
|
d % kinematic: link offset
|
|
alpha % kinematic: link twist
|
|
a % kinematic: link length
|
|
sigma % revolute=0, prismatic=1
|
|
mdh % standard DH=0, MDH=1
|
|
offset % joint coordinate offset
|
|
name % joint coordinate name
|
|
|
|
% dynamic parameters
|
|
m % dynamic: link mass
|
|
r % dynamic: position of COM with respect to link frame (3x1)
|
|
I % dynamic: inertia of link with respect to COM (3x3)
|
|
Jm % dynamic: motor inertia
|
|
B % dynamic: motor viscous friction (1x1 or 2x1)
|
|
|
|
Tc % dynamic: motor Coulomb friction (1x2 or 2x1)
|
|
G % dynamic: gear ratio
|
|
qlim % joint coordinate limits (2x1)
|
|
end
|
|
|
|
methods
|
|
function l = Link(varargin)
|
|
%LINK Create robot link object
|
|
%
|
|
% This is class constructor function which has several call signatures.
|
|
%
|
|
% L = Link() is a Link object with default parameters.
|
|
%
|
|
% L = Link(L1) is a Link object that is a deep copy of the link
|
|
% object L1.
|
|
%
|
|
% L = Link(OPTIONS) is a link object with the kinematic and dynamic
|
|
% parameters specified by the key/value pairs.
|
|
%
|
|
% Key/value pairs::
|
|
% 'theta',TH joint angle, if not specified joint is revolute
|
|
% 'd',D joint extension, if not specified joint is prismatic
|
|
% 'a',A joint offset (default 0)
|
|
% 'alpha',A joint twist (default 0)
|
|
% 'standard' defined using standard D&H parameters (default).
|
|
% 'modified' defined using modified D&H parameters.
|
|
% 'offset',O joint variable offset (default 0)
|
|
% 'qlim',L joint limit (default [])
|
|
% 'I',I link inertia matrix (3x1, 6x1 or 3x3)
|
|
% 'r',R link centre of gravity (3x1)
|
|
% 'm',M link mass (1x1)
|
|
% 'G',G motor gear ratio (default 0)
|
|
% 'B',B joint friction, motor referenced (default 0)
|
|
% 'Jm',J motor inertia, motor referenced (default 0)
|
|
% 'Tc',T Coulomb friction, motor referenced (1x1 or 2x1), (default [0 0])
|
|
% 'revolute' for a revolute joint (default)
|
|
% 'prismatic' for a prismatic joint 'p'
|
|
% 'standard' for standard D&H parameters (default).
|
|
% 'modified' for modified D&H parameters.
|
|
% 'sym' consider all parameter values as symbolic not numeric
|
|
%
|
|
% - It is an error to specify 'theta' and 'd'
|
|
% - The link inertia matrix (3x3) is symmetric and can be specified by giving
|
|
% a 3x3 matrix, the diagonal elements [Ixx Iyy Izz], or the moments and products
|
|
% of inertia [Ixx Iyy Izz Ixy Iyz Ixz].
|
|
% - All friction quantities are referenced to the motor not the load.
|
|
% - Gear ratio is used only to convert motor referenced quantities such as
|
|
% friction and interia to the link frame.
|
|
%
|
|
% Old syntax::
|
|
% L = Link(DH, OPTIONS) is a link object using the specified kinematic
|
|
% convention and with parameters:
|
|
% - DH = [THETA D A ALPHA SIGMA OFFSET] where OFFSET is a constant displacement
|
|
% between the user joint angle vector and the true kinematic solution.
|
|
% - DH = [THETA D A ALPHA SIGMA] where SIGMA=0 for a revolute and 1 for a
|
|
% prismatic joint, OFFSET is zero.
|
|
% - DH = [THETA D A ALPHA], joint is assumed revolute and OFFSET is zero.
|
|
%
|
|
% Options::
|
|
%
|
|
% 'standard' for standard D&H parameters (default).
|
|
% 'modified' for modified D&H parameters.
|
|
% 'revolute' for a revolute joint, can be abbreviated to 'r' (default)
|
|
% 'prismatic' for a prismatic joint, can be abbreviated to 'p'
|
|
%
|
|
% Examples::
|
|
% A standard Denavit-Hartenberg link
|
|
% L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2);
|
|
% since 'theta' is not specified the joint is assumed to be revolute, and
|
|
% since the kinematic convention is not specified it is assumed 'standard'.
|
|
%
|
|
% Using the old syntax
|
|
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard');
|
|
% the flag 'standard' is not strictly necessary but adds clarity.
|
|
%
|
|
% For a modified Denavit-Hartenberg link
|
|
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'modified');
|
|
%
|
|
% Notes::
|
|
% - Link object is a reference object, a subclass of Handle object.
|
|
% - Link objects can be used in vectors and arrays.
|
|
% - The parameter D is unused in a revolute joint, it is simply a placeholder
|
|
% in the vector and the value given is ignored.
|
|
% - The parameter THETA is unused in a prismatic joint, it is simply a placeholder
|
|
% in the vector and the value given is ignored.
|
|
% - The joint offset is a constant added to the joint angle variable before
|
|
% forward kinematics and subtracted after inverse kinematics. It is useful
|
|
% if you want the robot to adopt a 'sensible' pose for zero joint angle
|
|
% configuration.
|
|
% - The link dynamic (inertial and motor) parameters are all set to
|
|
% zero. These must be set by explicitly assigning the object
|
|
% properties: m, r, I, Jm, B, Tc, G.
|
|
|
|
%TODO eliminate legacy dyn matrix
|
|
|
|
if nargin == 0
|
|
% create an 'empty' Link object
|
|
% this call signature is needed to support arrays of Links
|
|
|
|
%% kinematic parameters
|
|
l.alpha = 0;
|
|
l.a = 0;
|
|
l.theta = 0;
|
|
l.d = 0;
|
|
l.sigma = 0;
|
|
l.mdh = 0;
|
|
l.offset = 0;
|
|
l.qlim = [];
|
|
|
|
%% dynamic parameters
|
|
% these parameters must be set by the user if dynamics is used
|
|
l.m = [];
|
|
l.r = [];
|
|
l.I = [];
|
|
|
|
% dynamic params with default (zero friction)
|
|
l.Jm = 0;
|
|
l.G = 0;
|
|
l.B = 0;
|
|
l.Tc = [0 0];
|
|
elseif nargin == 1 && isa(varargin{1}, 'Link')
|
|
% clone the passed Link object
|
|
l = copy(varargin{1});
|
|
|
|
else
|
|
% Create a new Link based on parameters
|
|
|
|
% parse all possible options
|
|
opt.theta = [];
|
|
opt.a = 0;
|
|
opt.d = [];
|
|
opt.alpha = 0;
|
|
opt.G = 0;
|
|
opt.B = 0;
|
|
opt.Tc = [0 0];
|
|
opt.Jm = [];
|
|
opt.I = [];
|
|
opt.m = [];
|
|
opt.r = [];
|
|
opt.offset = 0;
|
|
opt.qlim = [];
|
|
opt.type = {'revolute', 'prismatic', 'fixed'};
|
|
opt.convention = {'standard', 'modified'};
|
|
opt.sym = false;
|
|
|
|
[opt,args] = tb_optparse(opt, varargin);
|
|
|
|
% return a parameter as a number of symbol depending on
|
|
% the 'sym' option
|
|
|
|
if isempty(args)
|
|
% This is the new call format, where all parameters are
|
|
% given by key/value pairs
|
|
%
|
|
% eg. L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2);
|
|
if ~isempty(opt.theta)
|
|
% constant value of theta means it must be prismatic
|
|
l.theta = value( opt.theta, opt);
|
|
l.sigma = 1;
|
|
end
|
|
|
|
if ~isempty(opt.d)
|
|
% constant value of d means it must be revolute
|
|
l.d = value( opt.d, opt);
|
|
l.sigma = 0;
|
|
end
|
|
if ~isempty(opt.d) && ~isempty(opt.theta)
|
|
error('RTB:Link:badarg', 'specify only one of ''d'' or ''theta''');
|
|
end
|
|
|
|
l.a = value( opt.a, opt);
|
|
l.alpha = value( opt.alpha, opt);
|
|
|
|
l.offset = value( opt.offset, opt);
|
|
l.qlim = value( opt.qlim, opt);
|
|
|
|
l.m = value( opt.m, opt);
|
|
l.r = value( opt.r, opt);
|
|
l.I = value( opt.I, opt);
|
|
l.Jm = value( opt.Jm, opt);
|
|
l.G = value( opt.G, opt);
|
|
l.B = value( opt.B, opt);
|
|
l.Tc = value( opt.Tc, opt);
|
|
else
|
|
% This is the old call format, where all parameters are
|
|
% given by a vector containing kinematic-only, or
|
|
% kinematic plus dynamic parameters
|
|
%
|
|
% eg. L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard');
|
|
dh = args{1};
|
|
if length(dh) < 4
|
|
error('must provide params (theta d a alpha)');
|
|
end
|
|
|
|
% set the kinematic parameters
|
|
l.theta = dh(1);
|
|
l.d = dh(2);
|
|
l.a = dh(3);
|
|
l.alpha = dh(4);
|
|
|
|
l.sigma = 0;
|
|
l.offset = 0;
|
|
l.mdh = 0; % default to standard D&H
|
|
|
|
% optionally set sigma and offset
|
|
if length(dh) >= 5
|
|
l.sigma = dh(5);
|
|
end
|
|
if length(dh) == 6
|
|
l.offset = dh(6);
|
|
end
|
|
|
|
if length(dh) > 6
|
|
% legacy DYN matrix
|
|
|
|
l.sigma = dh(5);
|
|
l.mdh = 0; % default to standard D&H
|
|
l.offset = 0;
|
|
|
|
% it's a legacy DYN matrix
|
|
l.m = dh(6);
|
|
l.r = dh(7:9)'; % a column vector
|
|
v = dh(10:15);
|
|
l.I = [ v(1) v(4) v(6)
|
|
v(4) v(2) v(5)
|
|
v(6) v(5) v(3)];
|
|
if length(dh) > 15
|
|
l.Jm = dh(16);
|
|
end
|
|
if length(dh) > 16
|
|
l.G = dh(17);
|
|
else
|
|
l.G = 1;
|
|
end
|
|
if length(dh) > 17
|
|
l.B = dh(18);
|
|
else
|
|
l.B = 0.0;
|
|
end
|
|
if length(dh) > 18
|
|
l.Tc = dh(19:20);
|
|
else
|
|
l.Tc = [0 0];
|
|
end
|
|
l.qlim = [];
|
|
else
|
|
% we know nothing about the dynamics
|
|
l.m = [];
|
|
l.r = [];
|
|
l.I = [];
|
|
l.Jm = [];
|
|
l.G = 0;
|
|
l.B = 0;
|
|
l.Tc = [0 0];
|
|
l.qlim = [];
|
|
end
|
|
end
|
|
|
|
% set the kinematic convention to be used
|
|
if strcmp(opt.convention, 'modified')
|
|
l.mdh = 1;
|
|
else
|
|
l.mdh = 0;
|
|
end
|
|
|
|
end
|
|
|
|
function out = value(v, opt)
|
|
if opt.sym
|
|
out = sym(v);
|
|
else
|
|
out = v;
|
|
end
|
|
end
|
|
|
|
end % link()
|
|
|
|
function tau = friction(l, qd)
|
|
%Link.friction Joint friction force
|
|
%
|
|
% F = L.friction(QD) is the joint friction force/torque for link velocity QD.
|
|
%
|
|
% Notes::
|
|
% - Friction values are referred to the motor, not the load.
|
|
% - Viscous friction is scaled up by G^2.
|
|
% - Coulomb friction is scaled up by G.
|
|
% - The sign of the gear ratio is used to determine the appropriate
|
|
% Coulomb friction value in the non-symmetric case.
|
|
|
|
tau = l.B * abs(l.G) * qd;
|
|
|
|
if issym(l)
|
|
tau = tau + l.Tc;
|
|
elseif qd > 0
|
|
tau = tau + l.Tc(1);
|
|
elseif qd < 0
|
|
tau = tau + l.Tc(2);
|
|
end
|
|
tau = -abs(l.G) * tau; % friction opposes motion
|
|
end % friction()
|
|
|
|
function l2 = nofriction(l, only)
|
|
%Link.nofriction Remove friction
|
|
%
|
|
% LN = L.nofriction() is a link object with the same parameters as L except
|
|
% nonlinear (Coulomb) friction parameter is zero.
|
|
%
|
|
% LN = L.nofriction('all') as above except that viscous and Coulomb friction
|
|
% are set to zero.
|
|
%
|
|
% LN = L.nofriction('coulomb') as above except that Coulomb friction is set to zero.
|
|
%
|
|
% LN = L.nofriction('viscous') as above except that viscous friction is set to zero.
|
|
%
|
|
% Notes::
|
|
% - Forward dynamic simulation can be very slow with finite Coulomb friction.
|
|
%
|
|
% See also SerialLink.nofriction, SerialLink.fdyn.
|
|
|
|
l2 = Link(l);
|
|
|
|
if nargin == 1
|
|
only = 'coulomb';
|
|
end
|
|
|
|
switch only
|
|
case 'all'
|
|
l2.B = 0;
|
|
l2.Tc = [0 0];
|
|
case 'viscous'
|
|
l2.B = 0;
|
|
case 'coulomb'
|
|
l2.Tc = [0 0];
|
|
end
|
|
end
|
|
|
|
function v = RP(l)
|
|
%Link.RP Joint type
|
|
%
|
|
% c = L.RP() is a character 'R' or 'P' depending on whether joint is revolute or
|
|
% prismatic respectively.
|
|
% If L is a vector of Link objects return a string of characters in joint order.
|
|
v = '';
|
|
for ll=l
|
|
if ll.sigma == 0
|
|
v = strcat(v, 'R');
|
|
else
|
|
v = strcat(v, 'P');
|
|
end
|
|
end
|
|
end % RP()
|
|
|
|
function set.r(l, v)
|
|
%Link.r Set centre of gravity
|
|
%
|
|
% L.r = R set the link centre of gravity (COG) to R (3-vector).
|
|
%
|
|
if isempty(v)
|
|
return;
|
|
end
|
|
if length(v) ~= 3
|
|
error('RTB:Link:badarg', 'COG must be a 3-vector');
|
|
end
|
|
l.r = v(:)';
|
|
end % set.r()
|
|
|
|
function set.Tc(l, v)
|
|
%Link.Tc Set Coulomb friction
|
|
%
|
|
% L.Tc = F set Coulomb friction parameters to [F -F], for a symmetric
|
|
% Coulomb friction model.
|
|
%
|
|
% L.Tc = [FP FM] set Coulomb friction to [FP FM], for an asymmetric
|
|
% Coulomb friction model. FP>0 and FM<0.
|
|
%
|
|
% See also Link.friction.
|
|
if isempty(v)
|
|
return;
|
|
end
|
|
|
|
if isa(v,'sym') && ~isempty(findsym(v))
|
|
l.Tc = sym('Tc');
|
|
elseif isa(v,'sym') && isempty(findsym(v))
|
|
v = double(v);
|
|
end
|
|
|
|
if length(v) == 1 ~isa(v,'sym')
|
|
l.Tc = [v -v];
|
|
elseif length(v) == 2 && ~isa(v,'sym')
|
|
if v(1) < v(2)
|
|
error('RTB:Link:badarg', 'Coulomb friction is [Tc+ Tc-]');
|
|
end
|
|
l.Tc = v;
|
|
else
|
|
error('Coulomb friction vector can have 1 (symmetric) or 2 (asymmetric) elements only')
|
|
end
|
|
end % set.Tc()
|
|
|
|
function set.I(l, v)
|
|
%Link.I Set link inertia
|
|
%
|
|
% L.I = [Ixx Iyy Izz] set link inertia to a diagonal matrix.
|
|
%
|
|
% L.I = [Ixx Iyy Izz Ixy Iyz Ixz] set link inertia to a symmetric matrix with
|
|
% specified inertia and product of intertia elements.
|
|
%
|
|
% L.I = M set Link inertia matrix to M (3x3) which must be symmetric.
|
|
if isempty(v)
|
|
return;
|
|
end
|
|
if all(size(v) == [3 3])
|
|
if isa(v, 'double') && norm(v-v') > eps
|
|
error('inertia matrix must be symmetric');
|
|
end
|
|
l.I = v;
|
|
elseif length(v) == 3
|
|
l.I = diag(v);
|
|
elseif length(v) == 6
|
|
l.I = [ v(1) v(4) v(6)
|
|
v(4) v(2) v(5)
|
|
v(6) v(5) v(3) ];
|
|
else
|
|
error('RTB:Link:badarg', 'must set I to 3-vector, 6-vector or symmetric 3x3');
|
|
end
|
|
end % set.I()
|
|
|
|
function v = islimit(l, q)
|
|
%Link.islimit Test joint limits
|
|
%
|
|
% L.islimit(Q) is true (1) if Q is outside the soft limits set for this joint.
|
|
%
|
|
% Note::
|
|
% - The limits are not currently used by any Toolbox functions.
|
|
if isempty(l.qlim)
|
|
error('no limits assigned to link')
|
|
end
|
|
v = (q > l.qlim(2)) - (q < l.qlim(1));
|
|
end % islimit()
|
|
|
|
function v = isrevolute(L)
|
|
%Link.isrevolute Test if joint is revolute
|
|
%
|
|
% L.isrevolute() is true (1) if joint is revolute.
|
|
%
|
|
% See also Link.isprismatic.
|
|
|
|
v = L.sigma == 0;
|
|
end
|
|
|
|
function v = isprismatic(L)
|
|
%Link.isprismatic Test if joint is prismatic
|
|
%
|
|
% L.isprismatic() is true (1) if joint is prismatic.
|
|
%
|
|
% See also Link.isrevolute.
|
|
v = L.sigma == 1;
|
|
end
|
|
|
|
|
|
function T = A(L, q)
|
|
%Link.A Link transform matrix
|
|
%
|
|
% T = L.A(Q) is the link homogeneous transformation matrix (4x4) corresponding
|
|
% to the link variable Q which is either the Denavit-Hartenberg parameter 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.
|
|
sa = sin(L.alpha); ca = cos(L.alpha);
|
|
q = q + L.offset;
|
|
if L.sigma == 0
|
|
% revolute
|
|
st = sin(q); ct = cos(q);
|
|
d = L.d;
|
|
else
|
|
% prismatic
|
|
st = sin(L.theta); ct = cos(L.theta);
|
|
d = q;
|
|
end
|
|
|
|
if L.mdh == 0
|
|
% standard DH
|
|
|
|
T = [ ct -st*ca st*sa L.a*ct
|
|
st ct*ca -ct*sa L.a*st
|
|
0 sa ca d
|
|
0 0 0 1];
|
|
else
|
|
% modified DH
|
|
|
|
T = [ ct -st 0 L.a
|
|
st*ca ct*ca -sa -sa*d
|
|
st*sa ct*sa ca ca*d
|
|
0 0 0 1];
|
|
end
|
|
|
|
end % A()
|
|
|
|
function display(l)
|
|
%Link.display Display parameters
|
|
%
|
|
% L.display() displays the link parameters in compact single line format. If L is a
|
|
% vector of Link objects displays one line per element.
|
|
%
|
|
% Notes::
|
|
% - This method is invoked implicitly at the command line when the result
|
|
% of an expression is a Link object and the command has no trailing
|
|
% semicolon.
|
|
%
|
|
% See also Link.char, Link.dyn, SerialLink.showlink.
|
|
loose = strcmp( get(0, 'FormatSpacing'), 'loose');
|
|
if loose
|
|
disp(' ');
|
|
end
|
|
disp([inputname(1), ' = '])
|
|
disp( char(l) );
|
|
end % display()
|
|
|
|
function s = char(links, from_robot)
|
|
%Link.char Convert to string
|
|
%
|
|
% s = L.char() is a string showing link parameters in a compact single line format.
|
|
% If L is a vector of Link objects return a string with one line per Link.
|
|
%
|
|
% See also Link.display.
|
|
|
|
% display in the order theta d a alpha
|
|
if nargin < 2
|
|
from_robot = false;
|
|
end
|
|
|
|
s = '';
|
|
|
|
for j=1:length(links)
|
|
l = links(j);
|
|
|
|
conv = l.RP;
|
|
if l.mdh == 0
|
|
conv = strcat(conv, ',stdDH');
|
|
else
|
|
conv = strcat(conv, ',modDH');
|
|
end
|
|
if length(links) == 1
|
|
qname = 'q';
|
|
else
|
|
qname = sprintf('q%d', j);
|
|
end
|
|
|
|
if from_robot
|
|
if l.sigma == 1
|
|
% prismatic joint
|
|
js = sprintf('|%3d|%11s|%11s|%11s|%11s|', ...
|
|
j, ...
|
|
render(l.theta), ...
|
|
qname, ...
|
|
render(l.a), ...
|
|
render(l.alpha));
|
|
else
|
|
% revolute joint
|
|
js = sprintf('|%3d|%11s|%11s|%11s|%11s|', ...
|
|
j, ...
|
|
qname, ...
|
|
render(l.d), ...
|
|
render(l.a), ...
|
|
render(l.alpha));
|
|
end
|
|
else
|
|
|
|
if l.sigma == 1
|
|
% prismatic joint
|
|
js = sprintf(' theta=%s, d=%s, a=%s, alpha=%s (%s)', ...
|
|
render(l.theta), ...
|
|
qname, ...
|
|
render(l.a), ...
|
|
render(l.alpha), ...
|
|
conv);
|
|
else
|
|
% revolute
|
|
js = sprintf(' theta=%s, d=%s, a=%s, alpha=%s (%s)', ...
|
|
qname, ...
|
|
render(l.d), ...
|
|
render(l.a), ...
|
|
render(l.alpha), ...
|
|
conv);
|
|
end
|
|
end
|
|
if isempty(s)
|
|
s = js;
|
|
else
|
|
s = char(s, js);
|
|
end
|
|
end
|
|
|
|
function s = render(v, fmt)
|
|
if nargin < 2
|
|
fmt = '%11.4g';
|
|
end
|
|
if isa(v, 'double')
|
|
s = sprintf(fmt, v);
|
|
elseif isa(v, 'sym')
|
|
s = char(v);
|
|
else
|
|
error('RTB:Link:badarg', 'Link parameter must be numeric or symbolic');
|
|
end
|
|
end
|
|
end % char()
|
|
|
|
function dyn(links)
|
|
%Link.dyn Show 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 link.
|
|
%
|
|
% See also SerialLink.dyn.
|
|
|
|
for j=1:numel(links)
|
|
l = links(j);
|
|
if numel(links) > 1
|
|
fprintf('\nLink %d::', j);
|
|
end
|
|
fprintf('%s\n', l.char());
|
|
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
|
|
end
|
|
end % dyn()
|
|
|
|
% Make a copy of a handle object.
|
|
% http://www.mathworks.com/matlabcentral/newsreader/view_thread/257925
|
|
function new = copy(this)
|
|
|
|
for j=1:length(this)
|
|
% Instantiate new object of the same class.
|
|
%new(j) = feval(class(this(j)));
|
|
new(j) = Link();
|
|
% Copy all non-hidden properties.
|
|
p = properties(this(j));
|
|
for i = 1:length(p)
|
|
new(j).(p{i}) = this(j).(p{i});
|
|
end
|
|
end
|
|
end
|
|
|
|
function links = horzcat(this, varargin)
|
|
links = this.toLink();
|
|
|
|
for i=1:length(varargin)
|
|
links = cat(2, links, varargin{i}.toLink());
|
|
end
|
|
end
|
|
|
|
function links = vertcat(this, varargin)
|
|
links = this.horzcat(varargin{:});
|
|
end
|
|
|
|
function new = toLink(this)
|
|
for j=1:length(this)
|
|
new(j) = Link();
|
|
|
|
% Copy all non-hidden properties.
|
|
p = properties(this(j));
|
|
for i = 1:length(p)
|
|
new(j).(p{i}) = this(j).(p{i});
|
|
end
|
|
end
|
|
|
|
end
|
|
|
|
function res = issym(l)
|
|
res = isa(l.alpha,'sym');
|
|
end
|
|
|
|
function sl = sym(l)
|
|
|
|
sl = Link(l); % clone the link
|
|
|
|
if ~isempty(sl.theta)
|
|
sl.theta = sym(sl.theta);
|
|
end
|
|
if ~isempty(sl.d)
|
|
sl.d = sym(sl.d);
|
|
end
|
|
sl.alpha = sym(sl.alpha);
|
|
sl.a = sym(sl.a);
|
|
sl.offset = sym(sl.offset);
|
|
|
|
sl.I = sym(sl.I);
|
|
sl.r = sym(sl.r);
|
|
sl.m = sym(sl.m);
|
|
|
|
sl.Jm = sym(sl.Jm);
|
|
sl.G = sym(sl.G);
|
|
sl.B = sym(sl.B);
|
|
sl.Tc = sym(sl.Tc);
|
|
|
|
end
|
|
end % methods
|
|
end % class
|