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.
93 lines
3.0 KiB
93 lines
3.0 KiB
% Copyright (C) 1993-2013, 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/>.
|
|
%
|
|
% http://www.petercorke.com
|
|
|
|
%%begin
|
|
|
|
% In the field of robotics there are many possible ways of representing
|
|
% orientations of which the most common are:
|
|
% - orthonormal rotation matrices (3x3),
|
|
% - three angles (1x3), and
|
|
% - quaternions.
|
|
|
|
% A rotation of pi/2 about the x-axis can be represented as an orthonormal rotation
|
|
% matrix
|
|
|
|
R = rotx(pi/2)
|
|
% which we can see is a 3x3 matrix.
|
|
|
|
% Such a matrix has the property that it's columns (and rows) are sets of orthogonal
|
|
% unit vectors. The determinant of such a matrix is always 1
|
|
|
|
det(R)
|
|
|
|
% Let's create a more complex rotation
|
|
|
|
R = rotx(30, 'deg') * roty(50, 'deg') * rotz(10, 'deg')
|
|
% where this time we have specified the rotation angle in degrees.
|
|
|
|
% Any rotation can be expressed in terms of a single rotation about some axis
|
|
% in space
|
|
|
|
[theta,vec] = tr2angvec(R)
|
|
% where theta is the angle (in radians) and vec is unit vector representing the
|
|
% direction of the rotation axis.
|
|
|
|
% Commonly rotations are represented by Euler angles
|
|
|
|
eul = tr2eul(R)
|
|
% which are three angles such that R = rotz(a)*roty(b)*rotz(c), ie. the rotations
|
|
% required about the Z, then then the Y, then the Z axis.
|
|
|
|
% Rotations are also commonly represented by roll-pitch-yaw angles
|
|
|
|
rpy = tr2rpy(R)
|
|
% which are three angles such that R = rotx(r)*roty(p)*rotz(y), ie. the rotations
|
|
% required about the X, then then the Y, then the Z axis.
|
|
|
|
% We can investigate the effects of rotations about different axes
|
|
% using this GUI based demonstration. The menu buttons allow the rotation
|
|
% axes to be varied
|
|
% close the window when you are done.
|
|
tripleangle('rpy', 'wait')
|
|
|
|
% The final useful form is the quaternion which comprises 4 numbers. We can create
|
|
% a quaternion from an orthonormal matrix
|
|
|
|
q = Quaternion(R)
|
|
% where we can see that it comprises a scalar part and a vector part. To convert back
|
|
|
|
q.R
|
|
% which is the same of the value of R we determined above.
|
|
|
|
% Quaternions are a class and the orientations they represent can be compounded, just
|
|
% as we do with rotation matrices by multiplication.
|
|
|
|
% First we create two quaternions
|
|
|
|
q1 = Quaternion( rotx(pi/2) )
|
|
q2 = Quaternion( roty(pi/2) )
|
|
|
|
% then the rotation of q1 followed by q2 is simply
|
|
|
|
q1 * q2
|
|
|
|
% We can also take the inverse of a Quaternion
|
|
|
|
q1 * inv(q1)
|
|
% which results in a null rotation (zero vector part)
|