subsref.m

Robot tool box - provides many functions that are useful in robotics including such things as kinem

%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

% 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 v = subsref(l, s)
	if s(1).type  == '()'
		if l.mdh == 0,
			v = linktran([l.alpha l.A l.theta l.D l.sigma], ...
				s(1).subs{1}+l.offset);
		else
			v = mlinktran([l.alpha l.A l.theta l.D l.sigma], ...
				s(1).subs{1}+l.offset);
		end
	elseif s(1).type  ~= '.'
		error('only .field supported')
	else

		% NOTE WELL:  the following code can't use getfield() since
		% getfield()  uses this, and Matlab will crash!!

		el = char(s(1).subs);
		switch el,
		%%%%%%% kinematic parameters
		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;

		%%%%%%% joint limit support
		case 'qlim',
			v = l.qlim;
		case 'islimit',
			if isempty(l.qlim)
				error('no limits assigned to link')
			end
			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));

		%%%%%%% dynamic parameters
		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 'm',
			v = l.m;
		%%%%%%% legacy parameters
		case 'dh',
			v = [l.alpha l.A l.theta l.D l.sigma];
		case 'dyn',
			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(:)'];
		otherwise, disp('Unknown method')
		end
	end


%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.

%	Copyright (C) Peter Corke 1993
function t = linktran(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 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];

%MLINKTRAN	Compute the link transform from kinematic parameters
%
%	MLINKTRAN(alpha, an, theta, dn)
%	MLINKTRAN(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.

%	Copyright (C) Peter Corke 1993
function t = mlinktran(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];