Spring physics applied to quaternions using python

Question

I want to create a simple physics system in python that works on quaternions in a similar fashion as velocity/position. Its main goal is to simulate one object being dragged and try to catch up to another over time. The simulation uses 3 variables: k: the spring constant, d: a dampening factor, and m: the mass of the dragged object.

Using classic euler integration I can solve the positions with:

import numpy as np
from numpy.core.umath_tests import inner1d

# init simulation values
dt = 1./30. # delta t @ 30fps
k = 0.5 # Spring constant
d = 0.1 # Damping
m = 0.1 # Mass

p_ = np.array([0,0,0]) # initial position of the dragged object
p  = np.array([1,2,0]) # position to catch up to, in real life this value could change over time
v  = np.array([0,0,0]) # velocity buffer (init speed is assumed to be 0)

# Euler Integration
n = 200 # loop over 200 times just to see the values converge
for i in xrange(n):
    x  = (p-p_)
    F  = (k*x - d*v) / m # spring force
    v  = v + F * dt # update velocity
    p_ = p_ + v * dt # update dragged position

    print p_ # values oscillate and eventually stabilize to p
    
    
    

This works great for position. By changing k,d and m I can get a snappier/heavier results and overall I'm satisfied with how it feels:

Now I want to do the same thing with quaternions. Because I have no experience doing physics with quaternions, I went down the naive road and applied the same function with a few modifications to deal with quaternion flips and normalization.

# quaternion expressed as x,y,z,w
q_ = np.array([0., 0., 0., 1.]) # initial quaternion of the dragged object
q  = np.array([0.382683, 0., 0., 0.92388]) # quaternion to catch up to (equivalent to xyz euler rotation of 45,0,0 degrees)
v  = np.array([0.,0.,0.,0.]) # angular velocity buffer (init speed is assumed to be 0)

# Euler Integration
n = 200
for i in xrange(n):
    
    # In a real life use case, q varies over time and q_ tries to catch up to it.
    # Test to see if q and q_ are flipped with a dot product (innder1d).
    # If so, negate q
    if inner1d(q,q_) < 0:
        q = np.negative(q)
    
    x  = (q-q_)
    F  = (k*x - d*v) / m # spring force
    v  = v + F * dt # update velocity
    q_ = q_ + v * dt # update dragged quaternion
    q_ /= inner1d(q_,q_) # normalize
    
    print q_ # values oscillate and eventually stabilize to q

And to my great surprise it gives me very decent results!

Because I went with my instinct, I am sure my solution is flawed (like if q and q_ are opposed) and that there is a correct/better way to achieve what I want.

QUESTION:

What is the correct way to simulate spring forces on quaternions that takes into account (at minimum) the mass of the dragged object, a spring stiffness and a damping factor.

Actual python code would be greatly appreciated as I have great difficulty reading PhD papers :). Also for quaternion manipulations I usually refer to Christoph Gohlke's excellent library, but please feel free to use any others in your answer.

Answer 1

"Better" will be pretty subjective here actually.

You're kind of getting away with murdering the concept of quaternions, because your step size and displacements are small. Depending on your application, that might actually be ok (game engines often exploit tricks like this to make real-time calculations easier) but if your goal is accuracy, or you want to increase your step size and not get unstable results, you'll need to use quaternions as they are meant to be used.

As @z0r explained in the comments, since quaternions transform rotations by multiplication, the "difference" between them is the multiplicative inverse - basically, quaternion division.

qinv = quaternion_inverse(q)  # Using Gohlke's package 
x = quaternion_multiply(q_, qinv)

Now, much as for small theta, theta =~ sin(theta), this x is not very different from the result of subtraction as long as the difference is small. Abusing "small angle theorems" like this is used often in simulations of all types, but it is important to know when you're breaking them and what limitations it puts on your model.

Accelerations and velocities still add, so I think this still works:

F  = (k*x - d*v) / m 
v  = v + F * dt 

Compose the unit rotations

q_ = quaternion_multiply(q_, unit_vector(v * dt)) # update dragged quaternion

Once again, for small angles (i.e. dt is small compared to velocity), the sum and the product are very close.

Then normalize as before, if necessary.

q_ = unit_vector(q_)

I think that should work, but will be a bit slower than your previous version, and may have very similar results.

Answer 2

On the question "What is the correct way to simulate spring forces on quaternions", the reply is to write down the potential energy properly.

1. The quaternion gives you the orientation of the object via the operation

   orientation = vector_part(q_ r q_*)
   

   where the star denotes conjugation and r is a fixed orientation (say "unit vector along z" for instance, it must be unique in your system for all objects). The multiplication of q_, r and q_* is assumed to be "quaternion multiplication".

2. define the energy function with this orientation, for instance

   energy = dot_product(orientation, unit_z)
   

3. take "minus the derivative" of the energy with respect to the quaternion and you will have the force to apply to the system.

Your current code does a "damped oscillator in quaternion space" that might be a good solution to your problem, but it is not a spring force on an object :-)

PS: too long for a comment, I hope it helps. PS2: I didn't use a direct code for the problem above because (i) I didn't find it easy to just read in the library documentation above and (ii) the first part of the problem is to do the math/physics.