CODEWITHSUNDEEP
pythonquaternionscoordinate-systemseuler-angles
Eric
Eric

Reputation: 475

How to obtain the angle between two quaternions?

I am using inertial measurement units on the thigh and calf to try and obtain the knee joint angle in all degrees of freedom (x,y,z or flexion, adduction, rotation). An algorithm provided by the company converts the 9DOF from accelerometer, gyroscope, and magnetometer data to a quaternion for each sensor.

I am looking to get the x, y, and z (knee joint) angles between the thigh and calf sensors using the quaternions I am given.

I am pretty new to quaternions and I am trying to understand a way to do this through python but also conceptually understand it.

Upvotes: 4

Views: 15015

Answers (2)

Mauricio Cele Lopez Belon
Mauricio Cele Lopez Belon

Reputation: 1507

I think you can simply apply your quaternions Q1 and Q2 to the reference vectors X, Y, Z and then compute the angles between them. Like:

X1 = Q1*X*conj(Q1);
Y1 = Q1*Y*conj(Q1);
Z1 = Q1*Z*conj(Q1);

X2 = Q2*X*conj(Q2);
Y2 = Q2*Y*conj(Q2);
Z2 = Q2*Z*conj(Q2);


DiffAngleX = acos(dot(X1,X2));
DiffAngleY = acos(dot(Y1,Y2));
DiffAngleZ = acos(dot(Z1,Z2));

Upvotes: 1

Mr. Snrub
Mr. Snrub

Reputation: 342

Disclaimer: I'm pretty new to Quaternions myself but have done some work "near" them. The below is the result of my limited knowledge plus a few Google searches. It sure looks like it ought to do the trick.

So it sounds like the problem you're trying to solve can be stated as follows:

  • Given two quaternions (which represent the 3D orientation of the upper and lower leg, respectively)...
  • ...calculate the 3D angular difference between the two quaternions...
  • ... and represent that angular difference as Euler angles

To get the 3D angular difference, which itself is a quaternion, you just multiply one quaternion by the conjugate of the other (reference).

Then you need to convert from a quaternion to Euler angles (rotation about X, Y, Z). From what I can tell you'll need to do that The Old Fashioned Way, using the formulas from Wikipedia.

Sample code, using the pyquaternion library:

import pyquaternion as pyq
import math

# Create a hypothetical orientation of the upper leg and lower leg
# We use the (axis, degrees) notation because it's the most intuitive here
# Upper leg perfectly vertical with a slight rotation
q_upper = pyq.Quaternion(axis=[0.0, 0.0, -1.0], degrees=-5)
# Lower leg a little off-vertical, with a rotation in the other direction.
q_lower = pyq.Quaternion(axis=[0.1, -0.2, -0.975], degrees=10)

# Get the 3D difference between these two orientations
qd = q_upper.conjugate * q_lower

# Calculate Euler angles from this difference quaternion
phi   = math.atan2( 2 * (qd.w * qd.x + qd.y * qd.z), 1 - 2 * (qd.x**2 + qd.y**2) )
theta = math.asin ( 2 * (qd.w * qd.y - qd.z * qd.x) )
psi   = math.atan2( 2 * (qd.w * qd.z + qd.x * qd.y), 1 - 2 * (qd.y**2 + qd.z**2) )

# Result:
# phi   = 1.16 degrees
# theta = -1.90 degrees
# psi   = -14.77 degrees

Caveats:

  • I haven't hand-verified the correctness of this but it sure looks like it ought to be right.
  • You will of course want to be sure you verify the actual orientation and sign of each of the angles (phi, theta, psi) versus what you expect them to be.
  • In the Wikipedia article (in their C sample code) they add a little correction to the asin call for calculating theta. I'm not sure if that's needed. But if theta truly is adduction, I'm guessing you won't need to worry about angles above 90 degrees anyway ;-)

Upvotes: 4

Related Questions