www.gusucode.com > 机器人工具箱 - robot源码程序 > robot\@link\link.m
%LINK create a new LINK object % % A LINK object holds all information related to a robot link such as % kinematics of the joint, rigid-body inertial parameters, motor and % transmission parameters. % % L = LINK % L = LINK(link) % % Create a default link, or a clone of the passed link. % % A = LINK(q) % % Compute the link transform matrix for the link, given the joint % variable q. % % L = LINK([alpha A theta D]) % L =LINK([alpha A theta D sigma]) % L =LINK([alpha A theta D sigma offset]) % L =LINK([alpha A theta D], CONVENTION) % L =LINK([alpha A theta D sigma], CONVENTION) % L =LINK([alpha A theta D sigma offset], CONVENTION) % % If sigma or offset are not provided they default to zero. Offset is a % constant amount added to the joint angle variable before forward kinematics % and is useful if you want the robot to adopt a 'sensible' pose for zero % joint angle configuration. % % The optional CONVENTION argument is 'standard' for standard D&H parameters % or 'modified' for modified D&H parameters. If not specified the default % 'standard'. % Handling the different kinematic conventions is now hidden within the LINK % object. % % Conceivably all sorts of stuff could live in the LINK object such as % graphical models of links and so on. % % For robot models prior to Toolbox release 5 (pre Matlab objects) the % following object constructors are provided. % % L = LINK(DYN_ROW) create from row of legacy DYN matrix % L = LINK(DYN_ROW, CONVENTION) create from row of legacy DYN matrix % % SEE ALSO: LINK/SUBSREF, LINK/SUBSASGN, SHOWLINK, ROBOT. % Copyright (C) 1999-2008, 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/>. function l = link(dh, convention) if nargin == 0, % create an 'empty' link l.alpha = 0; l.A = 0; l.theta = 0; l.D = 0; l.sigma = 0; l.mdh = 0; l.offset = 0; % it's a legacy DYN matrix l.m = []; l.r = []; v = []; l.I = []; l.Jm = []; l.G = []; l.B = 0; l.Tc = [0 0]; l.qlim = []; l = class(l, 'link'); elseif isa(dh, 'link') % clone passed link l = dh; elseif length(dh) <= 6, % legacy DH matrix % link([alpha A theta D sigma]) if length(dh) < 4, error('must provide <alpha A theta D> params'); end l.alpha = dh(1); l.A = dh(2); l.theta = dh(3); l.D = dh(4); l.sigma = 0; if length(dh) >= 5, l.sigma = dh(5); end if nargin > 1 if strncmp(convention, 'mod', 3) == 1, l.mdh = 1; elseif strncmp(convention, 'sta', 3) == 1, l.mdh = 0; else error('convention must be modified or standard'); end else l.mdh = 0; % default to standard D&H end l.offset = 0; if length(dh) >= 6, l.offset = dh(6); end % we know nothing about the dynamics l.m = []; l.r = []; v = []; l.I = []; l.Jm = []; l.G = []; l.B = 0; l.Tc = [0 0]; l.qlim = []; l = class(l, 'link'); else % legacy DYN matrix l.alpha = dh(1); l.A = dh(2); l.theta = dh(3); l.D = dh(4); if length(dh) == 4, l.sigma = 0; else l.sigma = dh(5); end 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 = []; if nargin > 1 if strncmp(convention, 'mod', 3) == 1, l.mdh = 1; elseif strncmp(convention, 'sta', 3) == 1, l.mdh = 0; else error('convention must be modified or standard'); end else l.mdh = 0; % default to standard D&H end l = class(l, 'link'); end