Shortest {angle,axis} between 2 frames which we know their quaternion with respect to a world frame

94 Views Asked by At

I want to figure out what is the shortest angle between 2 frames (a frame is an orthonormal set of 3 vectors, all starting from a same point), assuming they have rotated around the same origin.

I read several topics, some using 2*acos, some 2*atan, but I cannot really find the shortest angle.

The ideas, perhaps wrong, are that :

  • q1 leads a starting frame (let's name it o_ini) to another orientation o1,
  • q2 leads the same starting frame to yet another orientation o2
  • we search q_diff leading o1 to o2, knowing only q1 and q2
  • some people say that it's such that q2 = q_diff*q1 and hence q_diff = q2 * inv(q1)
  • recalling that inv(q1) = conjugate(q1) as long as q1 is already a unit quaternion (all its 4 components, not only the 3 of the vector part)
  • then once q_diff is obtained, some naively use the definition of the scalar part (cos(angle/2)) to extract the angle with 2*acos(scalar part),
  • or some others use the fact that all the vector part components have in common the factor sin(angle/2) to use 2*atan2() as written here https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix

I've written a matlab (should works in the free software scilab) code illustrating why it does not suit me, doing some simple rotations around z and comparing the outputs with the simple difference of angles :

clear all
clc
z = [0;0;1];
toUnit=1;
angle1 = pi/4 ; angle2 = pi/2 ; angle3 = -4*pi/3;

h1 = quat(angle1,z,toUnit);
h2 = quat(angle2,z,toUnit);
h3 = quat(angle3,z,toUnit);
% all with respect to global frame

disp('With first implementation,')
fprintf(['shortest angle \n from after-h1-rotation configuration \n to ' ...
  'after-h2-rotation configuration = ', num2str(diffQuat1(h1,h2)*180/pi), '°. \n'])
fprintf(['shortest angle \n from after-h1-rotation configuration \n to ' ...
  'after-h3-rotation configuration = ', num2str(diffQuat1(h1,h3)*180/pi), '°. \n'])
disp('which is wrong.')

disp('With second implementation,')
fprintf(['shortest angle \n from after-h1-rotation configuration \n to ' ...
  'after-h2-rotation configuration = ', num2str(diffQuat2(h1,h2,0)*180/pi), '°. \n'])
fprintf(['shortest angle \n from after-h1-rotation configuration \n to ' ...
  'after-h3-rotation configuration = ', num2str(diffQuat2(h1,h3,0)*180/pi), '°. \n'])
disp('which is NOT ALWAYS true.')
disp('for q1=(45°,z) and q2= either (90°,z) or (-45°,z) it works,')
disp('but for q1=(45°,z) and q2=-4pi/3 it doesnot')

function h = quat(angle,axis,unit) %angle rad
%% build a quat from axis-angle representation
if unit ; axis = axis/norm(axis) ; end
if not(iscolumn(axis)) ; axis = axis'; end
h = [cos(angle/2);sin(angle/2)*axis]; h = h/norm(h);
end

function diffAngle = diffQuat1(h1,h2) 
%% should give angle from after-h1-rotation configuration to after-h2-rotation configuration
if iscolumn(h1) ; h1 = h1'; end
if iscolumn(h2) ; h2 = h2'; end
tmp = quatmultiply(h2,quatinv(h1));
diffAngle = 2*acos(tmp(4));
end

function diffAngle = diffQuat2(h1,h2,switchArg)
h1 = h1/norm(h1) ; h2 = h2/norm(h2);
if iscolumn(h1) ; h1 = h1'; end
if iscolumn(h2) ; h2 = h2'; end
if switchArg ; q1 = h2 ; q2 = h1;
else ; q1 = h1 ; q2 = h2; end
hdiff = quatmultiply(q2,quatinv(q1));
hdiff = hdiff/norm(hdiff);
diffAngle = 2*atan2(norm(hdiff(2:4)),hdiff(1));
end

I was told that using an atan2 with the ordinate argument being a norm (always positive then) makes us loose the potential sign~information of theta that the sin() could have brought, which is relevant I think, but then how to achieve my goal ? I saw nowhere a workaround, thanks