TR2EUL Convert a homogeneous transform matrix to Euler angle form [PHI THETA PSI] = TR2EUL(M) Returns a vector of roll/pitch/yaw angles corresponding to M, either a rotation matrix or the rotation part of a homogeneous transform. The 3 angles correspond to rotations about the Z, Y and Z axes respectively. See also: EUL2TR, TR2RPY
0001 %TR2EUL Convert a homogeneous transform matrix to Euler angle form 0002 % 0003 % [PHI THETA PSI] = TR2EUL(M) 0004 % 0005 % Returns a vector of roll/pitch/yaw angles corresponding to M, either a rotation 0006 % matrix or the rotation part of a homogeneous transform. 0007 % The 3 angles correspond to rotations about the Z, Y and Z axes respectively. 0008 % 0009 % See also: EUL2TR, TR2RPY 0010 0011 % Copyright (C) 1993-2008, by Peter I. Corke 0012 % 0013 % This file is part of The Robotics Toolbox for Matlab (RTB). 0014 % 0015 % RTB is free software: you can redistribute it and/or modify 0016 % it under the terms of the GNU Lesser General Public License as published by 0017 % the Free Software Foundation, either version 3 of the License, or 0018 % (at your option) any later version. 0019 % 0020 % RTB is distributed in the hope that it will be useful, 0021 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0022 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0023 % GNU Lesser General Public License for more details. 0024 % 0025 % You should have received a copy of the GNU Leser General Public License 0026 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0027 0028 function euler = tr2eul(m) 0029 0030 s = size(m); 0031 if length(s) > 2, 0032 euler = []; 0033 for i=1:s(3), 0034 euler = [euler; tr2eul(m(:,:,i))]; 0035 end 0036 return 0037 end 0038 0039 euler = zeros(1,3); 0040 0041 % Method as per Paul, p 69. 0042 % phi = atan2(ay, ax) 0043 % Only positive phi is returned. 0044 if abs(m(1,3)) < eps & abs(m(2,3)) < eps, 0045 % singularity 0046 euler(1) = 0; 0047 sp = 0; 0048 cp = 1; 0049 euler(2) = atan2(cp*m(1,3) + sp*m(2,3), m(3,3)); 0050 euler(3) = atan2(-sp * m(1,1) + cp * m(2,1), -sp*m(1,2) + cp*m(2,2)); 0051 else 0052 euler(1) = atan2(m(2,3), m(1,3)); 0053 sp = sin(euler(1)); 0054 cp = cos(euler(1)); 0055 euler(2) = atan2(cp*m(1,3) + sp*m(2,3), m(3,3)); 0056 euler(3) = atan2(-sp * m(1,1) + cp * m(2,1), -sp*m(1,2) + cp*m(2,2)); 0057 end