Home > . > tr2rpy.m

tr2rpy

PURPOSE ^

TR2RPY Convert a homogeneous transform matrix to roll/pitch/yaw angles

SYNOPSIS ^

function rpy = tr2rpy(m)

DESCRIPTION ^

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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003