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.
94 lines
3.0 KiB
94 lines
3.0 KiB
%TRCHAIN2 Chain 2D transforms from string
|
|
%
|
|
% T = TRCHAIN2(S, Q) is a homogeneous transform (3x3) that results from
|
|
% compounding a number of elementary transformations defined by the string
|
|
% S. The string S comprises a number of tokens of the form X(ARG) where
|
|
% X is one of Tx, Ty or R. ARG is the name of a variable in
|
|
% MATLAB workspace or qJ where J is an integer in the range 1 to N that
|
|
% selects the variable from the Jth column of the vector Q (1xN).
|
|
%
|
|
% For example:
|
|
% trchain('R(q1)Tx(a1)R(q2)Ty(a3)R(q3)', [1 2 3])
|
|
%
|
|
% is equivalent to computing:
|
|
% trot2(1) * transl2(a1,0) * trot2(2) * transl2(0,a3) * trot2(3)
|
|
%
|
|
% Notes::
|
|
% - The string can contain spaces between elements or on either side of ARG.
|
|
% - Works for symbolic variables in the workspace and/or passed in via the
|
|
% vector Q.
|
|
% - For symbolic operations that involve use of the value pi, make sure you
|
|
% define it first in the workspace: pi = sym('pi');
|
|
%
|
|
% See also trchain, trot2, transl2.
|
|
|
|
% Copyright (C) 1993-2015, 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 T = trchain2(s, q)
|
|
|
|
%s = 'R(q1)Tx(a1)R(q2)Tx(a3)R(q3)Tx(a3)';
|
|
|
|
tokens = regexp(s, '\s*(?<op>R.?|T.)\(\s*(?<arg>[A-Za-z][A-Za-z0-9]*)\s*\)\s*', 'names');
|
|
|
|
if isa(q, 'symfun')
|
|
q = formula(q);
|
|
end
|
|
|
|
T = eye(3,3);
|
|
joint = 1;
|
|
|
|
for token = tokens
|
|
|
|
% get the argument for this transform element
|
|
if token.arg(1) == 'q'
|
|
% from the passed in vector q
|
|
try
|
|
arg = q(joint);
|
|
catch
|
|
error('RTB:trchain2:badarg', 'vector q has insufficient values');
|
|
end
|
|
joint = joint+1;
|
|
else
|
|
% or the workspace
|
|
try
|
|
arg = evalin('base', token.arg);
|
|
catch
|
|
error('RTB:trchain2:badarg', 'variable %s does not exist', token.arg);
|
|
end
|
|
end
|
|
|
|
% now evaluate the element and update the transform chain
|
|
switch token.op
|
|
case 'R'
|
|
T = T * trot2(arg);
|
|
case 'Tx'
|
|
T = T * transl2(arg, 0);
|
|
case 'Ty'
|
|
T = T * transl2(0, arg);
|
|
otherwise
|
|
error('RTB:trchain2:badarg', 'unknown operator %s', token.op);
|
|
end
|
|
end
|
|
|
|
if isa(q, 'symfun')
|
|
T = formula(T);
|
|
end
|
|
|