# How can I convert Quaternions rotation values into Angular Velocity?

I am currently trying to teleoperate a UR5e robot using a haptic feedback device (3DSystems Touch). I have installed the necessary ROS(noetic) drivers for both devices.

I would like to be able to control the position of the robot's end effector using the position of the haptic feedback device's tip. After some research the moveit_servo library seemed appropriate for real-time teleoperation, but it appears to require me to publish Twist (so 3 degrees of linear and 3 degrees of angular velocity) to be able to work.

What I can currently read from the haptic feedback device (at 1000 Hz) is:

1. Current end effector pose relative to the device base (x, y, z in mm),
2. Current end effector orientation (in quaternions, unsure which frame this relates to but will learn),
3. Current end effector linear velocity (x, y, z in mm/s).

I believe I can put the linear velocity readings into the twist message as is (after some scaling and coordinate transformation) but I'm stumped on how to populate the angular velocity aspect. I intuitively feel that it should be computable from multiple readings of the orientation readings, but I am stumped as to how to do it.

Extra: I'm also open to suggestions for a better way of teleoperation.

It is possible to calculate x,y,z angular velocities from the time derivative of Quaternion. This blog has a detailed explanation for doing so: https://mariogc.com/post/angular-velocity-quaternions/

Quoting the relevant part of the code from this blog:

import numpy as np
def angular_velocities(q1, q2, dt):
return (2 / dt) * np.array([
q1[0]*q2[1] - q1[1]*q2[0] - q1[2]*q2[3] + q1[3]*q2[2],
q1[0]*q2[2] + q1[1]*q2[3] - q1[2]*q2[0] - q1[3]*q2[1],
q1[0]*q2[3] - q1[1]*q2[2] + q1[2]*q2[1] - q1[3]*q2[0]])


where q1 is the quaternion at time step t q2 is the quaternion after a delta time dt. This function returns an array of [ωx​,ωy,​​ωz​​] which you can substitute in the angular field of your command's Twist message.