Linked Questions

Popular Questions

Transformation between two coordinate frames (tf)

Asked by At

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.


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:

and also at the bottom of this post.


  • 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 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.

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.


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

      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')
          rospy.logerr(f"An exception of type {type(ex).__name__} occurred. Arguments:\n{ex.args}")

    if len(sys.argv) == 1:
      interval = 1/250
    elif len(sys.argv) == 2:
      interval = 1/float(sys.argv[1])
      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()
  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()
    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()
      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]
      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.

        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.

        TransformStamped: A TransformStamped message representing the transformation
            from the "world" frame to the child frame.
    transformation = TransformStamped()
    transformation.header.stamp =
    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 =
    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)
    # 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)
if __name__ == '__main__':
    rospy.init_node('fmp_tracepen_node', anonymous=True)
    tracepenNodeHandle = TracepenNode()
  except rospy.ROSInterruptException:

Related Questions