ROS: Transform one CS into another one

Question

you are my last hope. I am stuck in this for days now, and my remaining thesis time clock is ticking.

I have a robot manipulator arm and a tracepen (3D tracked device). I can get data from the tracepen using tracepenPose, which gives me a PoseStamped() object (let us call this frame tracepen_raw). This frame is not aligned with the world frame, rather shifted and rotated arbitrarily.

So my goal is to find a calibration which aligns the tracepen_raw frame with the world frame.

For this i want to have a Calibration() function. It should do the following:

On the manipulator, there is a calibration socket. Its position is known as tracepen_ok

rosrun tf tf_echo world tracepen_ok
At time 1683566330.781
- Translation: [0.999, 0.568, 0.198]
- Rotation: in Quaternion [0.006, 0.010, -0.004, 1.000]
            in RPY (radian) [0.012, 0.020, -0.007]
            in RPY (degree) [0.705, 1.135, -0.422]

The tracepen can be plugged into this socket.

Say i found the correct Calibration, then as long as the tracepen is in the socket, using Calibration(tracepen_raw) should give me tracepen_ok. Or with other words, in rviz, the tf of tracepen_calibrated should be the same as tracepen_raw, while the tracepen is plugged.

For this i came up with the following function:

  def calculate_calibration_matrix(self):
    input("Plug in Tracepen and press Enter")
    # get the tracepen_raw pose while plugged into the calibration socket
    raw_ok_pose = PoseStamped()
    raw_ok_pose = self.tracepenPose()
    
    # calculate a tf from world to tracepen_raw_plugged
    tf_tracepen_raw_plugged = TransformStamped()
    tf_tracepen_raw_plugged.header.stamp = rospy.Time.now()
    tf_tracepen_raw_plugged.header.frame_id = "world"
    tf_tracepen_raw_plugged.child_frame_id = "tracepen_raw_plugged"
    tf_tracepen_raw_plugged.transform.translation = raw_ok_pose.pose.position
    tf_tracepen_raw_plugged.transform.rotation = raw_ok_pose.pose.orientation
    
    # broadcaste the tf so we can lookup the transform later
    self.tf_broadcaster.sendTransform(tf_tracepen_raw_plugged)
    rospy.sleep(.2)
    
    # now get the transform we care about: the one from tracepen_ok to tracepen_raw_plugged. 
    # Applying it to raw data should give us calibrated data.
    calibration_tf = self.tf_buffer.lookup_transform("tracepen_ok", "tracepen_raw_plugged",  rospy.Time())

    # now calculate a calibration matrix from the tf
    self.calibrationMatrix = self.matrix_from_tf(calibration_tf)
    print(self.calibrationMatrix)
    

This should give me a homogeneous transformation which i can apply to a Pose Object to transform it into the world frame.

Therefore in the main loop:

  def run(self):
      rate = rospy.Rate(10)

      while not rospy.is_shutdown():

          # Get data from OpenVr
          tracepenPose = self.tracepenPose()
          
          # Get the tracepen_raw_tf and broadcast it
          tracepen_raw_tf = self.calculate_tf("tracepen_raw", tracepenPose)
          self.tf_broadcaster.sendTransform(tracepen_raw_tf)
          
          # Now convert the tracepen_raw_tf to a matrix, apply the calibration matrix...
          tracepen_calibrated_matrixnotation = self.calibrationMatrix @ self.matrix_from_tf(tracepen_raw_tf)
          
          # ... and convert it back to a tf
          tracepen_calibrated_tf = self.tf_from_matrix(tracepen_calibrated_matrixnotation, "world", "tracepen_calibrated")
          
          # Braodcast the calibrated tf.
          self.tf_broadcaster.sendTransform(tracepen_calibrated_tf)
        
          
          rate.sleep()

Except this does not do what it is suppoesed to do. Even in the plugged position, where actually tracepen_ok should coincide with tracepen_calibrated, it does not:

(The full code is:

#!/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.calculate_calibration_matrix()
    
    
  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, frameName, 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 = "world"
    transformation.child_frame_id = frameName
    transformation.transform.translation = pose.pose.position
    transformation.transform.rotation = pose.pose.orientation
    return transformation    
  
  
  
  def calculate_calibration_matrix(self):
    input("Plug in Tracepen and press Enter")
    # get the tracepen_raw pose while plugged into the calibration socket
    raw_ok_pose = PoseStamped()
    raw_ok_pose = self.tracepenPose()
    
    # calculate a tf from world to tracepen_raw_plugged
    tf_tracepen_raw_plugged = TransformStamped()
    tf_tracepen_raw_plugged.header.stamp = rospy.Time.now()
    tf_tracepen_raw_plugged.header.frame_id = "world"
    tf_tracepen_raw_plugged.child_frame_id = "tracepen_raw_plugged"
    tf_tracepen_raw_plugged.transform.translation = raw_ok_pose.pose.position
    tf_tracepen_raw_plugged.transform.rotation = raw_ok_pose.pose.orientation
    
    # broadcaste the tf so we can lookup the transform later
    self.tf_broadcaster.sendTransform(tf_tracepen_raw_plugged)
    rospy.sleep(.2)
    
    # now get the transform we care about: the one from tracepen_ok to tracepen_raw_plugged. 
    # Applying it to raw data should give us calibrated data.
    calibration_tf = self.tf_buffer.lookup_transform("tracepen_ok", "tracepen_raw_plugged",  rospy.Time())

    # now calculate a calibration matrix from the tf
    self.calibrationMatrix = self.matrix_from_tf(calibration_tf)
    print(self.calibrationMatrix)
    
    
  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_matrix = tf.transformations.quaternion_matrix(rotation) @ tf_matrix
    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 run(self):
      rate = rospy.Rate(10)

      while not rospy.is_shutdown():

          # Get data from OpenVr
          tracepenPose = self.tracepenPose()
          
          # Get the tracepen_raw_tf and broadcast it
          tracepen_raw_tf = self.calculate_tf("tracepen_raw", tracepenPose)
          self.tf_broadcaster.sendTransform(tracepen_raw_tf)
          
          # Now convert the tracepen_raw_tf to a matrix, apply the calibration matrix...
          tracepen_calibrated_matrixnotation = self.calibrationMatrix @ self.matrix_from_tf(tracepen_raw_tf)
          
          # ... and convert it back to a tf
          tracepen_calibrated_tf = self.tf_from_matrix(tracepen_calibrated_matrixnotation, "world", "tracepen_calibrated")
          
          # Braodcast the calibrated tf.
          self.tf_broadcaster.sendTransform(tracepen_calibrated_tf)
        
          
          rate.sleep()
          
  
      
if __name__ == '__main__':
  try:
    rospy.init_node('fmp_tracepen_node', anonymous=True)
    tracepenNodeHandle = TracepenNode()
    tracepenNodeHandle.run()
  except rospy.ROSInterruptException:
    pass

)

I do not think there are errors in the remaining functions. Probably my calibration sequence is missing something i just can think about.

I am extremely grateful for any response.

Many thanks in advance!