i am stuck with this problem for one week now.
My remaining master thesis time is slowly decresing. I would be so grateful for a bit of help.
EDIT
This new approach feels right, but still does not work.
I try to break down the problem into simple steps so it is easier to find the error. The full code is here: https://github.com/Infraviored/ThesisStuff/blob/main/tracepen_node_v2
and also at the bottom of this post.
Problem:
- I have a tracepen, a 3D tracked device.
- I want to have its Pose w.r.t. the world frame. So tf(world,tracepen) is my goal.
- I can get its Data in ROS as tracepenPose(), but this Pose is w.r.t. some unknown tracepenOrig. So i get Data of the form tf(tracepenOrig,tracepen) with unknown tracepenOrig.
- I want to find a calibration process which gives me the transformation between world and tracepen_Orig tf(world,tracepenOrig).
- After having found this, i can apply this calibration transformation to my tf(tracepenOrig,tracepen) to obtain tf(world,tracepen)
- I have a calibration socket which Pose i know relative to the world frame, so i know the tf(world,socket).
- When i plug in the tracepen into the calibration socket, the tf(world,tracepenPlugged) must be the same as tf(world,socket).
Solution attempt
- i pug in the tracepen to the socket
- i publish the Pose of the tracepen as tf(tracepenOrig,tracepenPlugged)
- i publish the transformation from world to socket as tf(world, socket)
- In plugged state, i know that tf(world,tracepenPlugged) must be the same as tf(world,socket)
- Or in as homogeneous transfomration: matrix_world_tracepenPlugged = matrix_world_socket
- I get the total homogeneous transformation from world to tracepenOrigin as
- matrix_world_tracepenOrigin = matrix_world_tracepenPlugged@ matrix_tracepenPlugged_tracepenOrigin
- from this i can publish the tf(world,tracepenOrigin)
- from this i can publish the tracepenPose() as tf(tracepenOrigin, tracepen)
It almost works. There is a tracepenOrigin frame published, with tracepen as child. While the tracepen is plugged in, the tracepen frame coincides with the socket frame.
But still: Moving the tracepen is off.
after calibration everything looks fine
after moving the socket x+0.2 (tracepen still plugged), the tracepen moves in another dircetion. It should still be in the socket. Also rotation of both frames is not the same.
CODE
really relevant are only the last two functions.
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped, TransformStamped, Pose
from std_msgs.msg import Bool, String
import tf2_ros
import tf
import numpy as np
import transforms3d
from transforms import Transforms3DUtils
from ur_msgs.msg import IOStates
#tracepen dependencies
import triad_openvr as triad_openvr
import time
import sys
import tf.transformations
class TracepenNode(object):
def __init__(self):
try:
self.v = triad_openvr.triad_openvr()
except Exception as ex:
if (type(ex).__name__ == 'OpenVRError' and ex.args[0] == 'VRInitError_Init_HmdNotFoundPresenceFailed (error number 126)'):
print('Cannot find the Tracepen.')
print('Is SteamVR running?')
print('Is the Tracepen turned on, connected, and paired with SteamVR?')
print('Are the Lighthouse Base Stations powered and in view of the tracepen?\n\n')
else:
rospy.logerr(f"An exception of type {type(ex).__name__} occurred. Arguments:\n{ex.args}")
quit()
if len(sys.argv) == 1:
interval = 1/250
elif len(sys.argv) == 2:
interval = 1/float(sys.argv[1])
else:
print("Invalid number of arguments")
interval = False
self.rpy = PoseStamped()
self.calibPose = PoseStamped()
self.rawPose = PoseStamped()
rospy.loginfo('fmp_tracepen_node: initialized')
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
print(self.waitForTracepen())
self.calibration()
def waitForTracepen(self):
""" Waits for the tracepen to be connected to the system
"""
tracepenPose = None
while tracepenPose is None or (tracepenPose.pose.position.x == 0 and tracepenPose.pose.position.y == 0 and tracepenPose.pose.position.z == 0):
tracepenPose = self.tracepenPose()
rospy.sleep(0.1)
return tracepenPose
def tracepenPose(self):
""" Get the data from the VR tracker through the steamVR
rparam position: The position of the tracepen already fixed in the x, y and z directions
rtype position: geometry_msgs.PoseStamped()
"""
tracepenpose = self.v.devices["controller_1"].getPoseQuaternion()
position = PoseStamped()
try:
position.pose.position.x = tracepenpose[0] #2
position.pose.position.y = tracepenpose[1] #0
position.pose.position.z = tracepenpose[2] #1
position.pose.orientation.x = tracepenpose[4]
position.pose.orientation.y = tracepenpose[5]
position.pose.orientation.z = tracepenpose[6]
position.pose.orientation.w = tracepenpose[3]
except:
rospy.logerr("fmp_tracepen_node: Data not received")
return position
def calculate_tf(self, parentFrame, childFrame, pose):
"""
Calculates a transformation from the "world" frame to a specified child frame,
based on a given PoseStamped message.
Args:
frameName (str): The name of the child frame for the transformation.
pose (PoseStamped): A PoseStamped message representing the pose of the child
frame relative to the "world" frame.
Returns:
TransformStamped: A TransformStamped message representing the transformation
from the "world" frame to the child frame.
"""
transformation = TransformStamped()
transformation.header.stamp = rospy.Time.now()
transformation.header.frame_id = parentFrame
transformation.child_frame_id = childFrame
transformation.transform.translation = pose.pose.position
transformation.transform.rotation = pose.pose.orientation
return transformation
def matrix_from_tf(self, tf_stamped):
translation = [tf_stamped.transform.translation.x, tf_stamped.transform.translation.y, tf_stamped.transform.translation.z]
rotation = [tf_stamped.transform.rotation.x, tf_stamped.transform.rotation.y, tf_stamped.transform.rotation.z, tf_stamped.transform.rotation.w]
tf_matrix = tf.transformations.translation_matrix(translation) @ tf.transformations.quaternion_matrix(rotation)
return tf_matrix
def tf_from_matrix(self, matrix, parent_frame, child_frame):
translation = tf.transformations.translation_from_matrix(matrix)
rotation = tf.transformations.quaternion_from_matrix(matrix)
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.now()
tf_stamped.header.frame_id = parent_frame
tf_stamped.child_frame_id = child_frame
tf_stamped.transform.translation.x = translation[0]
tf_stamped.transform.translation.y = translation[1]
tf_stamped.transform.translation.z = translation[2]
tf_stamped.transform.rotation.x = rotation[0]
tf_stamped.transform.rotation.y = rotation[1]
tf_stamped.transform.rotation.z = rotation[2]
tf_stamped.transform.rotation.w = rotation[3]
return tf_stamped
def calibration(self):
# I have a tracepen, a 3D tracked device. I can get its Data in ROS as tracepenPose(), but this Pose is w.r.t. some unknown_tracepenOrigin.
# So i want to find a calibration which transforms the data from this unknown frame to my world frame.
# Therefore i am looking for the tf(world,tracepenOrigin).
# The tracepenPose is w.r.t. some unknown tracepenOrigin. The tracepenPose is the transformation from the tracepenOriginin of this frame to the tracepen.
# As the tracepen is plugged, it is called tracepenPlugged.
rawPluggedPose = self.tracepenPose()
tf_tracepenOrigin_tracepenPlugged = self.calculate_tf("tracepenOrigin", "tracepenPlugged", rawPluggedPose)
self.tf_broadcaster.sendTransform(tf_tracepenOrigin_tracepenPlugged)
rospy.sleep(0.2)
# Look up the inverse trafo from tracepenPlugged to tracepenOrigin.
tf_tracepenPlugged_tracepenOrigin = self.tf_buffer.lookup_transform("tracepenPlugged", "tracepenOrigin", rospy.Time())
# Calculate the matrix from this tf.
matrix_tracepenPlugged_tracepenOrigin = self.matrix_from_tf(tf_tracepenPlugged_tracepenOrigin)
# I have a calibration socket which Pose i know relative to the world frame, so i know the tf(world,socket).
tf_world_socket = self.tf_buffer.lookup_transform("world", "socket", rospy.Time())
matrix_world_socket = self.matrix_from_tf(tf_world_socket)
# while plugged, i know that tf(world,tracepenPlugged) must be the same as tf(world,socket).
# so matrix_world_tracepenPlugged = matrix_world_socket
matrix_world_tracepenPlugged = matrix_world_socket
self.matrix_world_tracepenOrigin = matrix_world_tracepenPlugged @ matrix_tracepenPlugged_tracepenOrigin
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Get data from OpenVr
tracepenPose = self.tracepenPose()
# Publish the transformation from the world frame to the tracepenOrigin frame from the matrix of the calibration process.
self.tf_world_tracepenOrigin = self.tf_from_matrix(self.matrix_world_tracepenOrigin, "world", "tracepenOrigin")
self.tf_broadcaster.sendTransform(self.tf_world_tracepenOrigin)
#Use this tracepenOrigin to publish the tracepen Data.
tf_tracepenOrigin_tracepen = self.calculate_tf("tracepenOrigin", "tracepen", tracepenPose)
self.tf_broadcaster.sendTransform(tf_tracepenOrigin_tracepen)
rate.sleep()
if __name__ == '__main__':
try:
rospy.init_node('fmp_tracepen_node', anonymous=True)
tracepenNodeHandle = TracepenNode()
tracepenNodeHandle.run()
except rospy.ROSInterruptException:
pass