TR2RPY Convert a homogeneous transform matrix to roll/pitch/yaw angles [R P Y] = TR2RPY(M) Returns a vector of roll/pitch/yaw angles corresponding to M, either a rotation matrix or the rotation part of a homogeneous transform. part of the homogeneous transform TR. The angles correspond to rotations about the X, Y and Z axes respectively. See also: RPY2TR, TR2EUL
0001 %TR2RPY Convert a homogeneous transform matrix to roll/pitch/yaw angles 0002 % 0003 % [R P Y] = TR2RPY(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 % part of the homogeneous transform TR. The angles correspond to rotations 0008 % about the X, Y and Z axes respectively. 0009 % 0010 % See also: RPY2TR, TR2EUL 0011 0012 % Copyright (C) 1993-2008, by Peter I. Corke 0013 % 0014 % This file is part of The Robotics Toolbox for Matlab (RTB). 0015 % 0016 % RTB is free software: you can redistribute it and/or modify 0017 % it under the terms of the GNU Lesser General Public License as published by 0018 % the Free Software Foundation, either version 3 of the License, or 0019 % (at your option) any later version. 0020 % 0021 % RTB is distributed in the hope that it will be useful, 0022 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0023 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0024 % GNU Lesser General Public License for more details. 0025 % 0026 % You should have received a copy of the GNU Leser General Public License 0027 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0028 0029 function rpy = tr2rpy(m) 0030 0031 s = size(m); 0032 if length(s) > 2, 0033 rpy = []; 0034 for i=1:s(3), 0035 rpy = [rpy; tr2rpy(m(:,:,i))]; 0036 end 0037 return 0038 end 0039 rpy = zeros(1,3); 0040 0041 rpy(1) = atan2(-m(2,3), m(3,3)); % roll 0042 % compute sin/cos of roll angle 0043 sr = sin(rpy(1)); 0044 cr = cos(rpy(1)); 0045 rpy(2) = atan2(m(1,3), cr * m(3,3) - sr * m(2,3)); % pitch 0046 rpy(3) = atan2(-m(1,2), m(1,1)); % yaw