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.
78 lines
2.2 KiB
78 lines
2.2 KiB
%TRINTERP Interpolate homogeneous transformations
|
|
%
|
|
% T = TRINTERP(T0, T1, S) is a homogeneous transform interpolation
|
|
% between T0 when S=0 to T1 when S=1. Rotation is interpolated using
|
|
% quaternion spherical linear interpolation. If S (Nx1) then T (4x4xN)
|
|
% is a sequence of homogeneous transforms corresponding to the interpolation
|
|
% values in S.
|
|
%
|
|
% T = TRINTERP(T, S) is a transform that varies from the identity matrix when
|
|
% S=0 to T when R=1. If S (Nx1) then T (4x4xN) is a sequence of homogeneous
|
|
% transforms corresponding to the interpolation values in S.
|
|
%
|
|
% See also CTRAJ, QUATERNION.
|
|
|
|
|
|
% 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/>.
|
|
|
|
function T = trinterp(A, B, C)
|
|
|
|
if nargin == 3
|
|
% TR = TRINTERP(T0, T1, r)
|
|
T0 = A; T1 = B; r = C;
|
|
|
|
if length(r) > 1
|
|
T = [];
|
|
for rr=r(:)'
|
|
TT = trinterp(T0, T1, rr);
|
|
T = cat(3, T, TT);
|
|
end
|
|
return;
|
|
end
|
|
|
|
q0 = Quaternion(T0);
|
|
q1 = Quaternion(T1);
|
|
|
|
p0 = transl(T0);
|
|
p1 = transl(T1);
|
|
|
|
qr = q0.interp(q1, r);
|
|
pr = p0*(1-r) + r*p1;
|
|
elseif nargin == 2
|
|
% TR = TRINTERP(T, r)
|
|
T0 = A; r = B;
|
|
|
|
if length(r) > 1
|
|
T = [];
|
|
for rr=r(:)'
|
|
TT = trinterp(T0, rr);
|
|
T = cat(3, T, TT);
|
|
end
|
|
return;
|
|
end
|
|
|
|
q0 = Quaternion(T0);
|
|
p0 = transl(T0);
|
|
|
|
qr = q0.scale(r);
|
|
pr = r*p0;
|
|
else
|
|
error('must be 2 or 3 arguments');
|
|
end
|
|
T = rt2tr(qr.R, pr);
|
|
|