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:
- Current end effector pose relative to the device base (x, y, z in mm),
- Current end effector orientation (in quaternions, unsure which frame this relates to but will learn),
- 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.