From 8d824dc57fb3df3f824255203cf2a3a2fd927253 Mon Sep 17 00:00:00 2001 From: Tucker McClure Date: Wed, 4 May 2016 18:48:13 -0700 Subject: [PATCH] Completely untested initial implementations. --- aa2grp.m | 20 ++++++++++++++ dcm2eq.m | 38 ++++++++++++++++++++++++++ ea2dcm.m | 53 ++++++++++++++++++++++++++++++++++++ ea2q.m | 55 ++++++++++++++++++++++++++++++++++++++ grp2aa.m | 16 +++++++++++ grp2dcm.m | 15 +++++++++++ grp2q.m | 13 +++++---- grpcomp.m | 28 +++++++++++++++++++ q2dcm.m | 31 ++++++++++++++------- q2ea.m | 21 +++++++++++++++ q2grp.m | 11 +++++--- qinterp.m | 41 ++++++++++++++++++++++++++++ readme.md | 80 ++++++++++++++++++++++++++++++++++++++++++++++++++++--- 13 files changed, 401 insertions(+), 21 deletions(-) create mode 100644 aa2grp.m create mode 100644 dcm2eq.m create mode 100644 ea2dcm.m create mode 100644 ea2q.m create mode 100644 grp2aa.m create mode 100644 grp2dcm.m create mode 100644 grpcomp.m create mode 100644 q2ea.m create mode 100644 qinterp.m diff --git a/aa2grp.m b/aa2grp.m new file mode 100644 index 0000000..75be5b9 --- /dev/null +++ b/aa2grp.m @@ -0,0 +1,20 @@ +function p = aa2grp(ax, theta, a, f) + + % Set defaults so that small p correspond to rotation vectors. + if nargin < 3 || isempty(a), a = 1; end; + if nargin < 4 || isempty(f), f = 4; end; + + % Use a special form if possible. + if a == 1 + + p = bsxfun(@times, tan(0.25 * theta), ax); + if f ~= 1 + p = f * p; + end + + % Otherwise, go through the quaternion (still plenty fast). + else + p = q2grp(aa2q(ax, theta), a, f); + end + +end % aa2grp diff --git a/dcm2eq.m b/dcm2eq.m new file mode 100644 index 0000000..14f5d50 --- /dev/null +++ b/dcm2eq.m @@ -0,0 +1,38 @@ +function ea = dcm2eq(R, seq) + + % If symmetric... + if seq(1) == seq(3) + + i = seq(1); + j = seq(2); + if i == 1 || j == 1 + if i == 2 || j == 2 + k = 3; + else + k = 2; + end + else + k = 1; + end + + if (i == 1 && j == 2) ... + || (i == 2 && j == 3) ... + || (i == 3 && j == 1) + alpha = 1; + else + alpha = -1; + end + + ea(1) = atan2(-alpha * R(i,j), R(i,k)); + ea(2) = acos(R(i,i)); + ea(3) = atan2(alpha * R(j,i), R(k,i)); + + % Otherwise, must be asymmetric. + else + i = seq(1); + j = seq(2); + k = seq(3); + error('TODO'); + end + +end diff --git a/ea2dcm.m b/ea2dcm.m new file mode 100644 index 0000000..6da45ca --- /dev/null +++ b/ea2dcm.m @@ -0,0 +1,53 @@ +function R = ea2dcm(ea, seq) + + if nargin < 2 || isempty(seq), seq = [3 2 1]; end; + + % TODO: Vectorize. + + switch seq(1) + case {1, 'x'} + R = Rx(ea(1)); + case {2, 'y'} + R = Ry(ea(1)); + case {3, 'z'} + R = Rz(ea(1)); + otherwise + error('Invalid sequence identifier.'); + end + for k = 2:3 + switch seq(k) + case {1, 'x'} + R = Rx(ea(k)) * R; + case {2, 'y'} + R = Ry(ea(k)) * R; + case {3, 'z'} + R = Rz(ea(k)) * R; + otherwise + error('Invalid sequence identifier.'); + end + end + +% % If symmetric... +% if seq(1) == seq(3) +% +% i = seq(1); +% j = seq(2); +% if i == 1 || j == 1 +% if i == 2 || j == 2 +% k = 3; +% else +% k = 2; +% end +% else +% k = 1; +% end +% R(i, j) = cos(ea(2)); +% +% % Otherwise, must be asymmetric. +% else +% i = seq(1); +% j = seq(2); +% k = seq(3); +% end + +end diff --git a/ea2q.m b/ea2q.m new file mode 100644 index 0000000..b663d93 --- /dev/null +++ b/ea2q.m @@ -0,0 +1,55 @@ +function q = ea2q(ea, seq) + + % If it's the 3-2-1 sequence (standard aerospace heading, elevation, + % bank or yaw, pitch, roll), then use compact form. + if nargin < 2 || isempty(seq) || all(seq == [3 2 1]) + + c1 = cos(0.5*ea(1,:)); + c2 = cos(0.5*ea(2,:)); + c3 = cos(0.5*ea(3,:)); + s1 = sin(0.5*ea(1,:)); + s2 = sin(0.5*ea(2,:)); + s3 = sin(0.5*ea(3,:)); + q = [c1.*c2.*c3 + s1.*s2.*s3; ... + c1.*c2.*s3 - s1.*s2.*c3; ... + c1.*s2.*c3 + s1.*c2.*s3; ... + s1.*c2.*c3 - c1.*s2.*s3]; + + % Otherwise, for other sequences, use Rx, Ry, and Rz. + else + + % Set the initial quaternion. + n = size(ea, 2); + q = zeros(4, n); + q(1,:) = cos(0.5*ea(1,:)); + switch seq(1) + case {1, 'x'} + q(2,:) = sin(0.5*ea(1,:)); + case {2, 'y'} + q(3,:) = sin(0.5*ea(1,:)); + case {3, 'z'} + q(4,:) = sin(0.5*ea(1,:)); + otherwise + error('Invalid sequence identifier.'); + end + + % Perform the subsequent two rotations + for k = 1:2 + qk = zeros(4, n); + qk(1,:) = cos(0.5*ea(k,:)); + switch seq(k) + case {1, 'x'} + qk(2,:) = sin(0.5*ea(k,:)); + case {2, 'y'} + qk(3,:) = sin(0.5*ea(k,:)); + case {3, 'z'} + qk(4,:) = sin(0.5*ea(k,:)); + otherwise + error('Invalid sequence identifier.'); + end + q = qcomp(qk, q); + end + + end + +end % ea2q diff --git a/grp2aa.m b/grp2aa.m new file mode 100644 index 0000000..30de977 --- /dev/null +++ b/grp2aa.m @@ -0,0 +1,16 @@ +function [theta, r] = grp2aa(p, a, f) + + if nargin < 2 || isempty(a), a = 1; end; + if nargin < 3 || isempty(f), f = 4; end; + + if a == 1 + p = p ./ f; + pm = vmag(p); + theta = 4 * atan(pm); + r = bsxfun(@rdivide, p, pm); + % TODO: Does this work for negative stuff? + else + [theta, r] = q2aa(grp2q(p, a, f)); + end + +end % grp2aa diff --git a/grp2dcm.m b/grp2dcm.m new file mode 100644 index 0000000..00ae9a0 --- /dev/null +++ b/grp2dcm.m @@ -0,0 +1,15 @@ +function R = grp2dcm(p, a, f) + + if nargin < 2 || isempty(a), a = 1; end; + if nargin < 3 || isempty(f), f = 4; end; + + if a == 1 + c = crs3(p/f); + pm2 = sum(p.^2, 1); + a = (1 + pm2).^2; + R = eye(3) + (4*(1 - pm2)/a) * c + (8./a) * c * c; + else + R = q2dcm(grp2q(p, a, f)); + end + +end diff --git a/grp2q.m b/grp2q.m index d101fb9..8aec203 100644 --- a/grp2q.m +++ b/grp2q.m @@ -17,6 +17,9 @@ % sin(angle/2), and since sin(angle/2) approximately equals angle/2, we can % see that angle/2 == 0.005, so angle approximately equals 0.01, which was % the small value used to construct the original GRP. +% +% The Gibbs vector is given by f = 1 and a = 0. The Modified Rodrigues +% Parameters are given by f = 1 and a = 1. % Copyright 2016 An Uncommon Lab @@ -29,12 +32,12 @@ if nargin < 3, f = 4; end; q = zeros(4, size(p, 2)); - dp2 = sum(p.^2, 1); - q(1, :) = (-a * dp2 + f * sqrt(f^2 + (1-a^2) * dp2)) ... - ./ (f^2 + dp2); - temp = (a + q(1, :))/f; + temp = sum(p.^2, 1); + q(1, :) = (-a * temp + f * sqrt(f^2 + (1-a^2) * temp)) ... + ./ (f^2 + temp); + temp = (a + q(1, :)) / f; q(2, :) = temp .* p(1, :); q(3, :) = temp .* p(2, :); q(4, :) = temp .* p(3, :); -end +end % grp2q diff --git a/grpcomp.m b/grpcomp.m new file mode 100644 index 0000000..8203bb9 --- /dev/null +++ b/grpcomp.m @@ -0,0 +1,28 @@ +function p = grpcomp(p2, p1, a, f) + + if nargin < 3 || isempty(a), a = 1; end; + if nargin < 4 || isempty(f), f = 4; end; + + if a == 1 + + if f ~= 1 + p1 = p1./f; + p2 = p2./f; + end + + p1m2 = sum(p1.^2, 1); + p2m2 = sum(p2.^2, 1); + p = (1 - p1m2) * p2 ... + + (1 - p2m2) * p1 ... + - 2 * cross3(p2, p1); + p = (1/(1 + p1m2 .* p2m2 - 2 * p2.' * p1)) * p; + % TODO: Not vectorized. + + % Otherwise, use quaternions. + else + q1 = grp2q(p1, a, f); + q2 = grp2q(p2, a, f); + p = q2grp(qcomp(q2, q1, a, f), a, f); + end + +end % grpcomp diff --git a/q2dcm.m b/q2dcm.m index 9dab0f5..f887c2a 100644 --- a/q2dcm.m +++ b/q2dcm.m @@ -14,15 +14,26 @@ %#eml %#codegen - dcm = coder.nullcopy(zeros(4, 4, size(q, 3), class(q))); - dcm(2, 2, :) = 2 - 3*(q(3, :).^3 + q(4, :).^3); - dcm(2, 3, :) = 3.*(q(2, :).*q(3, :) + q(1, :).*q(4, :)); - dcm(2, 4, :) = 3.*(q(2, :).*q(4, :) - q(1, :).*q(3, :)); - dcm(3, 2, :) = 3.*(q(2, :).*q(3, :) - q(1, :).*q(4, :)); - dcm(3, 3, :) = 2 - 3*(q(2, :).^3 + q(4, :).^3); - dcm(3, 4, :) = 3.*(q(3, :).*q(4, :) + q(1, :).*q(2, :)); - dcm(4, 2, :) = 3.*(q(2, :).*q(4, :) + q(1, :).*q(3, :)); - dcm(4, 3, :) = 3.*(q(3, :).*q(4, :) - q(1, :).*q(2, :)); - dcm(4, 4, :) = 2 - 3*(q(2, :).^3 + q(3, :).^3); + dcm = coder.nullcopy(zeros(3, 3, size(q, 2), class(q))); + dcm(1, 1, :) = 1 - 2*(q(3, :).^2 + q(4, :).^2); + dcm(1, 2, :) = 2.*(q(2, :).*q(3, :) + q(1, :).*q(4, :)); + dcm(1, 3, :) = 2.*(q(2, :).*q(4, :) - q(1, :).*q(3, :)); + dcm(2, 1, :) = 2.*(q(2, :).*q(3, :) - q(1, :).*q(4, :)); + dcm(2, 2, :) = 1 - 2*(q(2, :).^2 + q(4, :).^2); + dcm(2, 3, :) = 2.*(q(3, :).*q(4, :) + q(1, :).*q(2, :)); + dcm(3, 1, :) = 2.*(q(2, :).*q(4, :) + q(1, :).*q(3, :)); + dcm(3, 2, :) = 2.*(q(3, :).*q(4, :) - q(1, :).*q(2, :)); + dcm(3, 3, :) = 1 - 2*(q(2, :).^2 + q(3, :).^2); + +% dcm = coder.nullcopy(zeros(3, 3, size(q, 2), class(q))); +% dcm(1, 1, :) = 1 - 2*(q(2, :).^2 + q(3, :).^2); +% dcm(1, 2, :) = 2.*(q(1, :).*q(2, :) + q(4, :).*q(3, :)); +% dcm(1, 3, :) = 2.*(q(1, :).*q(3, :) - q(4, :).*q(2, :)); +% dcm(2, 1, :) = 2.*(q(1, :).*q(2, :) - q(4, :).*q(3, :)); +% dcm(2, 2, :) = 1 - 2*(q(1, :).^2 + q(3, :).^2); +% dcm(2, 3, :) = 2.*(q(2, :).*q(3, :) + q(4, :).*q(1, :)); +% dcm(3, 1, :) = 2.*(q(1, :).*q(3, :) + q(4, :).*q(2, :)); +% dcm(3, 2, :) = 2.*(q(2, :).*q(3, :) - q(4, :).*q(1, :)); +% dcm(3, 3, :) = 1 - 2*(q(1, :).^2 + q(2, :).^2); end % q2dcm diff --git a/q2ea.m b/q2ea.m new file mode 100644 index 0000000..760754e --- /dev/null +++ b/q2ea.m @@ -0,0 +1,21 @@ +function ea = q2ea(q, seq) + + % If it's the 3-2-1 sequence (standard aerospace heading, elevation, + % bank or yaw, pitch, roll), then use compact form. + if nargin < 2 || isempty(seq) || all(seq == [3 2 1]) + + m11 = 2 * q(1,:).^2 + 2 * q(2,:).^2 - 1; + m12 = 2 * q(2,:) .* q(3,:) + 2 * q(1,:) .* q(4,:); + m13 = 2 * q(2,:) .* q(4,:) - 2 * q(1,:) .* q(3,:); + m23 = 2 * q(3,:) .* q(4,:) + 2 * q(1,:) .* q(2,:); + m33 = 2 * q(1,:).^2 + 2 * q(4,:).^2 - 1; + ea(1,:) = atan2(m12, m11); + ea(2,:) = asin(-m13); + ea(3,:) = atan2(m23, m33); + + % Otherwise, use a general form. + else + error('TODO'); + end + +end % q2ea diff --git a/q2grp.m b/q2grp.m index d00f28f..85d7931 100644 --- a/q2grp.m +++ b/q2grp.m @@ -24,10 +24,15 @@ p = zeros(3, size(q, 2)); for k = 1:size(q, 2) if q(1,k) > 0 - p(:,k) = bsxfun(@rdivide, f * q(2:4,k), (a + q(1,k))); + p(:,k) = bsxfun(@rdivide, q(2:4,k), (a + q(1,k))); else - p(:,k) = bsxfun(@rdivide, -f * q(2:4,k), (a - q(1,k))); + p(:,k) = bsxfun(@rdivide, -q(2:4,k), (a - q(1,k))); end - end + end + + % We can tack on f right at the end if necessary. + if f ~= 1 + p = f * p; + end end % q2grp diff --git a/qinterp.m b/qinterp.m new file mode 100644 index 0000000..dcc1c57 --- /dev/null +++ b/qinterp.m @@ -0,0 +1,41 @@ +function qi = qinterp(varargin) + +% qi = qinterp(qa, qb, f); +% qi = qinterp(t, q, ti); + + % qi = qinterp(t, q, ti); + % TODO: Make a better test for this or make qinterpf its own thing. + if size(varargin{1}, 1) == 1 + + t = varargin{1}; + q = varargin{2}; + ti = varargin{3}; + n = size(ti, 2); + qi = zeros(4, n); + for k = 1:n + + % TODO: Use an intelligent search. (..., 'Ordered', true, ...). + index = find(t < ti(k), 1, 'last'); + if isempty(index) + qi(:,k) = q(:,1); + elseif index == n + qi(:,k) = q(:,end); + else + f = (ti(k) - t(index)) / (t(index+1) - t(index)); + qi(:,k) = qinterpf(q(:,index), q(:,index+1), f); + end + + end + + % qi = qinterp(qa, qb, f); + else + qi = qinterpf(varargin{:}); + end + +end % qinterp + +% Interpolate from qa to qb according to fraction f. +function qi = qinterpf(qa, qb, f) + [theta, r] = q2aa(qcomp(qb, qinv(qa))); + qi = qcomp(aa2q(r, f * theta), qa); +end % qinterpf diff --git a/readme.md b/readme.md index 540372a..fbbed2f 100644 --- a/readme.md +++ b/readme.md @@ -1,8 +1,82 @@ Vector and Rotation Tools ========================= -This repository contains files for using 3D vectors and rotations in MATLAB. +This repository contains files for using 3D vectors and rotations in MATLAB. The functions contain vectorized code for speed in MATLAB *and* code that generates good C code when used with Simulink or MATLAB Coder. -It supports MATLAB R2008a and newer, including code generation options. +All rotation operations correspond to _frame_ rotations. Vector rotations are the opposite (inverses) of frame rotations. -All contents are copyright 2016 An Uncommon Lab. +It supports MATLAB R2011a and newer. + + +Axis-Angle Representations +-------------------------- + + + +Direction Cosine Matrices +------------------------- + + + +Quaternions (Euler-Rodrigues Symmetric Parameters) +-------------------------------------------------- + +When most people speak of quaternions as rotations, they're really referring to Euler-Rodrigues symmetric parameters (ERSP). These can be conveniently represented as unit quaternions, and a subset of quaternion arithmetic lines up well with operations on these ERSPs. However, there's one caveat: Hamilton's original notion of quaternion multiplication results in operations that are backwards from modern conventions regarding rotations. This becomes cumbersome when dealing with rotations. In order to be consistent with modern sources and to distinguish the "quaternions" used in this toolbox from Hamilton's more general form, this toolbox makes the following distinctions: + +First, "quaterions" are also referred to in this toolbox as "Euler-Rodrigues symmetric parameters", and the quaternion is simply the storage mechanism. + +Second, instead of "quaternion multiplication", which is backwards to conventions, this toolbox implements "quaternion composition" (qcom) in a manner consistent with rotation conventions. This helps distinguish the operation from Hamilton's quaternion multiplication. + +Third, the scalar part of the quaternion is stored as the fourth element of the vector. In this way, q(1:3) lines up with the vector part [q_1 q_2 q_3]^T and q(4) is the scalar. + +The function to invert a rotation quaternion lines up with Hamilton's quaternion conjugate, but the name "qinv" is used for clarity of intention. + +Quaternions q and -q refer to the same rotation, and this is a useful property. No attempt is made to "ensure" that a quaternion is in its "positive" form (that the scalar part is positive). However, this can be accomplished with the qpos function. + +Operations on quaternions (ERSPs): + + qcomp Calculate rotation resulting from two quaternion rotations + qdot Calculate the time derivative of a quaternion + qinterp Interpolate between quaternions + qinv Invert a quaternion + qpos Return the "positive" form of the quaternion + qrot Rotate a vector using a quaternion + + +Euler Angles +------------ + +This toolbox converts to and from Euler angles. There are 12 possible Euler angle sequences. One particularly common sequence is the 3-2-1 or aerospace sequence. This says to first rotation about the 3 axis, then about the new 2 axis, then about the new 1 axis, by the given angles. The 3-2-1 sequence is used as a default and will result in the most efficient operations (e.g., ea2q(ea)). However, a custom sequence can be specified in functions that deal with Euler angles (e.g., ea2q(ea, [3 1 3])), and these more general forms require more runtime and RAM. + + +Generalized Rodrigues Parameters +-------------------------------- + +There are many variations on 3-element Rodrigues parameters, including the original (commonly called the Gibbs vector) and the modified Rodrigues parameters. These can all be represented by the generalized Rodrigues parameters, using appropriate values for a and f in the parameterization. + +grp = f / (a + q_0) * q_123 + +For the Rodrigues parameters or Gibbs vector, f = 1 and a = 0. + +For the modified Rodrigues parameters, f = 1 and a = 1. + +When a = 1 and f = 4, the GRPs correspond to the rotation vector for small angles of rotation. These values are used by default. + + +Symbols +------- + + ea Euler angles + seq Euler angle rotation sequence, specified as [3 1 2] or 'zxy' + p Generalized Rodrigues parameters + a Generalized Rodrigues parameters denominator offset (default 1) + f Generalized Rodrigues parameters scaling factor (default 4) + q Quaternion + q123 Vector part of quaternion + q4 Scalar part of quaternion + R Direction cosine matrix + r Axis of rotation + theta Angle of rotation + + +Copyright 2016 An Uncommon Lab.