SUBSREF Reference methods on a LINK object LINK(q) return link transform (A) matrix LINK.alpha return DH parameters LINK.A LINK.theta LINK.D LINK.sigma return prismatic flag LINK.RP return 'R' or 'P' LINK.mdh 0 if standard D&H, else 1 LINK.offset return joint variable offset LINK.qlim return joint variable limits [min max] LINK.islimit(q) return if limit is exceeded: -1, 0, +1 LINK.I return 3x3 symmetric inertia matrix LINK.m return link mass LINK.r return 3x1 link COG vector LINK.G return gear ratio LINK.Jm return motor inertia LINK.B return viscous friction LINK.Tc return viscous friction LINK.dh return legacy DH row LINK.dyn return legacy DYN row
0001 %SUBSREF Reference methods on a LINK object 0002 % 0003 % LINK(q) return link transform (A) matrix 0004 % 0005 % LINK.alpha return DH parameters 0006 % LINK.A 0007 % LINK.theta 0008 % LINK.D 0009 % LINK.sigma return prismatic flag 0010 % LINK.RP return 'R' or 'P' 0011 % LINK.mdh 0 if standard D&H, else 1 0012 % 0013 % LINK.offset return joint variable offset 0014 % LINK.qlim return joint variable limits [min max] 0015 % LINK.islimit(q) return if limit is exceeded: -1, 0, +1 0016 % 0017 % LINK.I return 3x3 symmetric inertia matrix 0018 % LINK.m return link mass 0019 % LINK.r return 3x1 link COG vector 0020 % 0021 % LINK.G return gear ratio 0022 % LINK.Jm return motor inertia 0023 % LINK.B return viscous friction 0024 % LINK.Tc return viscous friction 0025 % 0026 % LINK.dh return legacy DH row 0027 % LINK.dyn return legacy DYN row 0028 0029 % Copyright (C) 1999-2008, by Peter I. Corke 0030 % 0031 % This file is part of The Robotics Toolbox for Matlab (RTB). 0032 % 0033 % RTB is free software: you can redistribute it and/or modify 0034 % it under the terms of the GNU Lesser General Public License as published by 0035 % the Free Software Foundation, either version 3 of the License, or 0036 % (at your option) any later version. 0037 % 0038 % RTB is distributed in the hope that it will be useful, 0039 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0040 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0041 % GNU Lesser General Public License for more details. 0042 % 0043 % You should have received a copy of the GNU Leser General Public License 0044 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0045 0046 function v = subsref(l, s) 0047 if s(1).type == '()' 0048 if l.mdh == 0, 0049 v = linktran([l.alpha l.A l.theta l.D l.sigma], ... 0050 s(1).subs{1}+l.offset); 0051 else 0052 v = mlinktran([l.alpha l.A l.theta l.D l.sigma], ... 0053 s(1).subs{1}+l.offset); 0054 end 0055 elseif s(1).type ~= '.' 0056 error('only .field supported') 0057 else 0058 0059 % NOTE WELL: the following code can't use getfield() since 0060 % getfield() uses this, and Matlab will crash!! 0061 0062 el = char(s(1).subs); 0063 switch el, 0064 %%%%%%% kinematic parameters 0065 case 'alpha', 0066 v = l.alpha; 0067 case 'A', 0068 v = l.A; 0069 case 'theta', 0070 v = l.theta; 0071 case 'D', 0072 v = l.D; 0073 case 'offset', 0074 v = l.offset; 0075 case 'sigma', 0076 v = l.sigma; 0077 case 'RP', 0078 if l.sigma == 0, 0079 v = 'R'; 0080 else 0081 v = 'P'; 0082 end 0083 case 'mdh', 0084 v = l.mdh; 0085 0086 %%%%%%% joint limit support 0087 case 'qlim', 0088 v = l.qlim; 0089 case 'islimit', 0090 if isempty(l.qlim) 0091 error('no limits assigned to link') 0092 end 0093 if s(2).type ~= '()' 0094 error('expecting argument for islimit method'); 0095 end 0096 q = s(2).subs{1}; 0097 v = (q > l.qlim(2)) - (q < l.qlim(1)); 0098 0099 %%%%%%% dynamic parameters 0100 case 'G', 0101 v = l.G; 0102 case 'I', 0103 v = l.I; 0104 case 'r', 0105 v = l.r; 0106 case 'Jm', 0107 v = l.Jm; 0108 case 'B', 0109 v = l.B; 0110 case 'Tc', 0111 v = l.Tc; 0112 0113 case 'm', 0114 v = l.m; 0115 %%%%%%% legacy parameters 0116 case 'dh', 0117 v = [l.alpha l.A l.theta l.D l.sigma]; 0118 case 'dyn', 0119 v = [l.alpha l.A l.theta l.D l.sigma l.m l.r(:)' diag(l.I)' l.I(2,1) l.I(2,3) l.I(1,3) l.Jm l.G l.B l.Tc(:)']; 0120 otherwise, disp('Unknown method') 0121 end 0122 end 0123 0124 0125 %LINKTRAN Compute the link transform from kinematic parameters 0126 % 0127 % LINKTRAN(alpha, an, theta, dn) 0128 % LINKTRAN(DH, q) is a homogeneous 0129 % transformation between link coordinate frames. 0130 % 0131 % alpha is the link twist angle 0132 % an is the link length 0133 % theta is the link rotation angle 0134 % dn is the link offset 0135 % sigma is 0 for a revolute joint, non-zero for prismatic 0136 % 0137 % In the second case, q is substitued for theta or dn according to sigma. 0138 % 0139 % Based on the standard Denavit and Hartenberg notation. 0140 0141 % Copyright (C) Peter Corke 1993 0142 function t = linktran(a, b, c, d) 0143 0144 if nargin == 4, 0145 alpha = a; 0146 an = b; 0147 theta = c; 0148 dn = d; 0149 else 0150 if numcols(a) < 4, 0151 error('too few columns in DH matrix'); 0152 end 0153 alpha = a(1); 0154 an = a(2); 0155 if numcols(a) > 4, 0156 if a(5) == 0, % revolute 0157 theta = b; 0158 dn = a(4); 0159 else % prismatic 0160 theta = a(3); 0161 dn = b; 0162 end 0163 else 0164 theta = b; % assume revolute if sigma not given 0165 dn = a(4); 0166 end 0167 end 0168 sa = sin(alpha); ca = cos(alpha); 0169 st = sin(theta); ct = cos(theta); 0170 0171 t = [ ct -st*ca st*sa an*ct 0172 st ct*ca -ct*sa an*st 0173 0 sa ca dn 0174 0 0 0 1]; 0175 0176 %MLINKTRAN Compute the link transform from kinematic parameters 0177 % 0178 % MLINKTRAN(alpha, an, theta, dn) 0179 % MLINKTRAN(DH, q) is a homogeneous 0180 % transformation between link coordinate frames. 0181 % 0182 % alpha is the link twist angle 0183 % an is the link length 0184 % theta is the link rotation angle 0185 % dn is the link offset 0186 % sigma is 0 for a revolute joint, non-zero for prismatic 0187 % 0188 % In the second case, q is substitued for theta or dn according to sigma. 0189 % 0190 % Based on the modified Denavit and Hartenberg notation. 0191 0192 % Copyright (C) Peter Corke 1993 0193 function t = mlinktran(a, b, c, d) 0194 0195 if nargin == 4, 0196 alpha = a; 0197 an = b; 0198 theta = c; 0199 dn = d; 0200 else 0201 if numcols(a) < 4, 0202 error('too few columns in DH matrix'); 0203 end 0204 alpha = a(1); 0205 an = a(2); 0206 if numcols(a) > 4, 0207 if a(5) == 0, % revolute 0208 theta = b; 0209 dn = a(4); 0210 else % prismatic 0211 theta = a(3); 0212 dn = b; 0213 end 0214 else 0215 theta = b; % assume revolute if no sigma given 0216 dn = a(4); 0217 end 0218 end 0219 sa = sin(alpha); ca = cos(alpha); 0220 st = sin(theta); ct = cos(theta); 0221 0222 t = [ ct -st 0 an 0223 st*ca ct*ca -sa -sa*dn 0224 st*sa ct*sa ca ca*dn 0225 0 0 0 1];