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.


1 Answer 1


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.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.