# Transformation between two coordinate frames (tf)

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 time

import sys

import tf.transformations

class TracepenNode(object):
def __init__(self):

try:
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()

self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

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.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.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)
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")

#Use this tracepenOrigin to publish the tracepen Data.
tf_tracepenOrigin_tracepen = self.calculate_tf("tracepenOrigin", "tracepen", tracepenPose)

rate.sleep()

if __name__ == '__main__':
try:
rospy.init_node('fmp_tracepen_node', anonymous=True)
tracepenNodeHandle = TracepenNode()
tracepenNodeHandle.run()
except rospy.ROSInterruptException:
pass
``````