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:

- quat_hand.rotate(mathutils.Euler((0.0,new_shoulder_angle, 0.0), ‘XYZ’))
- 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