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
```