Coordinate transformation with quaternions VQS

Hello everybody, I hope someone can give me a hand with this.
Imagine a single link “robotic arm” the shoulder is fixed and floating in midair and can rotate around the Y axis. The end-effector (hand) attached to the extreme of that arm does nothing but I want to know the position of a ball moving in the environment with respect to the hand. So I’m defining point A (0,0,0) as my world coordinate system for which I know the position of the ball (ball.worldPosition); next point B (0,0,10) as the point where the shoulder is attached; and finally point C (5,0,10) the point where the initial position of the hand is.

Using the transformation matrix method I managed to get all the data I need like this:

## ---------- Initialize matrices ---------
matrix_shoulder_T = mathutils.Matrix.Translation((0.0,0.0,-10.0))
matrix_shoulder_R = mathutils.Matrix.Identity(4)
matrix_shoulder = matrix_shoulder_T * matrix_shoulder_R

matrix_hand_T = mathutils.Matrix.Translation((-5.0,0.0,0.0))
matrix_hand_R = mathutils.Matrix.Identity(4)
matrix_hand = matrix_hand_T * matrix_hand_R
## -------------------------------------------
## ---- Get position of target wrt shoulder -----
ball_wrt_shoulder = matrix_shoulder * ball.worldPosition

## --- Update rotation matrix for reference frame C ---
aux0 = matrix_hand_R.to_euler()
aux0.y = new_shoulder_angle
matrix_hand_R = aux0.to_matrix().to_4x4()
matrix_hand = matrix_hand_T * matrix_hand_R
ballFX = matrix_hand * mat_shoulder * gBall.worldPosition

## ---- Get position of target wrt hand ----- this works, HURRA!!! 
ball_wrt_hand = matrix_hand * matrix_shoulder * ball.worldPosition

Since I’m planning to expand the dimensionality of this problem to several degrees of freedom I wanted to explore how to obtain the same information using quaternions instead of matrices but I’m doing something wrong and can’t figure out what it is.

## ---------- Initialize matrices ---------
quat_shoulder = mathutils.Quaternion((0.0, 0.0, 1.0), 0.0)
vect_shoulder = mathutils.Vector((0.0, 0.0, -10.0))

quat_hand = mathutils.Quaternion((0.0, 0.0, 1.0), 0.0)
vect_hand = mathutils.Vector((-5.0, 0.0, 0.0))
## -------------------------------------------
## ---- Get position of target wrt shoulder ----- this part works, YEAY!!! :o
ball_wrt_shoulder = quat_shoulder * quat_shoulder.inverted() * ball.worldPosition + vect_shoulder

## --- Update quaternion for reference frame C --- doesn't work :(
aux = quat_hand.to_euler()
aux.y = new_shoulder_angle
quat_hand = aux.to_quaternion()

## ---- Get position of target wrt hand ----- my holy grail :(
ball_wrt_hand = quat_hand * quat_hand.inverted() * ball_wrt_shoulder + vect_hand

I’ve tried several ways of changing quat_hand:

  1. quat_hand.rotate(mathutils.Euler((0.0,new_shoulder_angle, 0.0), ‘XYZ’))
  2. quat_hand = quat_hand * mathutils.Quaternion((0.0, 1.0, 0.0), new_shoulder_angle)
    the quaternions change but I don’t get the right results.
    Thanks in advance for any help

I am not an expert on quaternions, but I see some weird stuff in your code:

  • You are constructing the quat_shoulder and quat_hand quaternions from axis-angle as mathutils.Quaternion((0.0, 0.0, 1.0), 0.0), which is just the identity matrix (angle=0 => no rotation at all).
  • quat_X * quat_X.inverted() will also yield the identity matrix, regardless of the value of quat_X.

In general, problems like these may be easier to solve by parenting than by explicit matrix calculations.

Hi Sjoerd,
first thanks for replying…
about your first point, I’m not an expert on Quaternions either so I’m just following the API and that’s how they suggest to initialize a quaternion. I initialize it at angle = 0 but when running the game that angle will change according to the position of the ball.
about your second point, the formulation for coordinate transformation in VQS is P’ = S*(QPQ^-1) + V. I’m not using any scaling so I focus on translation and rotation. When I do this multiplication (QPQ^-1) Blender gives me an error and tells me to switch the order of the inverse (Q^-1) and the vector P. That’s the only way how it works without errors… when I put Q*(Q^-1 * P) I obtain the same result.

I don’t think that the quaternion rotation formula is commutative (i.e. you can’t move the terms around). You have the right formula:
p’ = q * p * q^-1

ball_wrt_shoulder = quat_shoulder * quat_shoulder.inverted() * ball.worldPosition + vect_shoulder

This is probably just canceling out your quat multiplications since:
ball_wrt_shoulder = q * q^-1 * p = (q * q^-1) * p = p

If you really want to go quats, try this:

ball_wrt_shoulder =   (quat_shoulder.inverted() * (quat_shoulder * ball.worldPosition)) + vect_shoulder

However, I don’t think you have to worry about the rotations yourself. Personally, I would just parent an Empty to the hand and get the ball’s relative location just by doing:

ball_relative_position = ball.worldPosition - handEmpty.worldPosition

Hi Kastoria,
I missed the part of something times its inverse is always identity… in any case your suggestion didn’t work either :frowning:
I tried the order you suggested and also having the inverted inside the parenthesis instead of the normal quaternion… but the end result is the same, there is no rotation!
I’m doing this because I do research in robotics so I’m trying to simulate a robotic platform and the rest of conditions as if I was in a real environment which means that I don’t have sensors in each joint (I can’t put an Empty to the hand) telling me positions and angles with respect to all possible targets… and my research is about presenting an alternative to the traditional inverse kinematics approach… which means that I don’t want to use the IK of Blender.
That’s why I’m trying to go this “manual” into the rotation and translation of points.
Hope you can help me solve why I can’t make a rotation with quaternions.
Thanks in advance

Ah! Blender is being ‘fancy’. The Quaternion class provided by Blender has changed the multiply operator (*) so that simply doing “Quaternion * Vector” applies the rotation. That means that when you do this:

q = Quaternion((0,0,1), pi/4)
v = Vector( (1, 0, 0) )
print(q * v)

That does what you current think requires “q * v * q^-1”.

But another though is why bother with quats at all. For a robotics application, it seems like euler angles fit the problem space more naturally than quats do. One of the main reasons to use quats is to avoid gimbal lock but in this case I would think you want to see that unless your robot has true ball-and-socket joints.

Thanks kastoria, that was it… it’s now working and it’s rotating and updating all the values I want as it should.
There is only a “stetic” part I’d like to solve and it is that on my main loop I can only rotate the quaternion using a conversion from quaternion to euler and then back to quaternion. I’d like to just do something like quaternion.rotate(Euler) but it doesn’t work… at least not in the main loop, the strange thing is that this DOES work if I’m not inside the main loop, so I’m breaking my head about why this is so… the code is exactly the same :confused:
Thanks again for your help
PS: You are right, in this kind of robotic configuration I don’t need to go quaternion but I have a different project where I use a gyroscope and your help is going to be very useful :wink:

How do I mark this thread as SOLVED?

Edit the top post in advanced mode.

The quick “solved” flag was lost during the recent hacker attack.

Thanks Monster, it seems it was a really bad attack… maybe it’s not related or maybe it is but I can’t login using Chrome, only Explorer. I appreciate any help/suggestion about this too.

You want to manage robotics you say ?

6dof joints are your friend,

each item can report it’s position if it’s not parented,

I have been working on a IK system for rigid bodies using Torque.

Note - Force = Torque applied to match a objects orientaiton

X1-Force applied to match a target position

(I know I need to re-write it to be less silly)


RoboCatch.blend (483 KB)

Hi BluePrintRandom,
thanks for sharing your code and ideas… it’s a bit difficult to understand so I’d really appreciate if you could give me more details. I’m impressed and I’d love to know more about how you do this. Since this thread is not directly related to force control I’ll send you a message in private.
Thanks again.