TR2DIFF Convert a transform difference to differential representation D = TR2DIFF(T) D = TR2DIFF(T1, T2) First form converts a homogeneous transform representing an infinitessimal motion to a 6-element differential representation. Such a homogeneous transform has a rotational submatrix that is, approximately, skew symmetric. Second form returns the 6-element differential motion required to move from T1 to T2 in base coordinates. See also: DIFF2TR.
0001 %TR2DIFF Convert a transform difference to differential representation 0002 % 0003 % D = TR2DIFF(T) 0004 % D = TR2DIFF(T1, T2) 0005 % 0006 % First form converts a homogeneous transform representing an 0007 % infinitessimal motion to a 6-element differential representation. 0008 % Such a homogeneous transform has a rotational submatrix that is, 0009 % approximately, skew symmetric. 0010 % 0011 % Second form returns the 6-element differential motion required to move 0012 % from T1 to T2 in base coordinates. 0013 % 0014 % See also: DIFF2TR. 0015 0016 % Copyright (C) 1993-2008, by Peter I. Corke 0017 % 0018 % This file is part of The Robotics Toolbox for Matlab (RTB). 0019 % 0020 % RTB is free software: you can redistribute it and/or modify 0021 % it under the terms of the GNU Lesser General Public License as published by 0022 % the Free Software Foundation, either version 3 of the License, or 0023 % (at your option) any later version. 0024 % 0025 % RTB is distributed in the hope that it will be useful, 0026 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0027 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0028 % GNU Lesser General Public License for more details. 0029 % 0030 % You should have received a copy of the GNU Leser General Public License 0031 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0032 0033 function d = tr2diff(t1, t2) 0034 if nargin == 1, 0035 d = [ t1(1:3,4); 0036 0.5*[t1(3,2)-t1(2,3); t1(1,3)-t1(3,1); t1(2,1)-t1(1,2)]]; 0037 else 0038 d = [ t2(1:3,4)-t1(1:3,4); 0039 0.5*( cross(t1(1:3,1), t2(1:3,1)) + ... 0040 cross(t1(1:3,2), t2(1:3,2)) + ... 0041 cross(t1(1:3,3), t2(1:3,3)) ... 0042 )]; 0043 end 0044