PROBLEM SOLVED

The previous issue arose due to an incorrect configuration of the tracepen matrix, which was not right-handed. However, I have successfully resolved this problem by extracting the rotation and adjusting the orientation of the x and y axes accordingly. Consequently, the correct origin has been determined, and it remains unaffected by the position of the calibration socket.

Moreover, this updated function incorporates improved error handling mechanisms to ensure smoother operation.

The function:

def tracepenMatrix(self):
    """
    Returns the transformation matrix of the tracepen device as a homogeneous 4x4 matrix.

    This method retrieves the pose matrix from the controller, converts it into
    a numpy array, and inserts it into the first three rows of a 4x4 identity matrix.
    The resulting matrix represents the pose of the tracepen in homogeneous coordinates.

    The transformation matrix consists of a 3x3 rotation matrix and a 1x3 translation vector. 
    The rotation matrix is flipped along the z-axis to change the frame handedness.

    Returns:
        np.ndarray or None: A 4x4 homogeneous transformation matrix representing the pose of the tracepen.
                            If the tracepen device is not available, it returns None.

    Raises:
        TypeError: If the pose matrix from the controller is not properly formed.
    """
    try:
        # Fetch pose matrix from controller device
        raw_tracepen_matrix = np.array(list(self.v.devices["controller_1"].get_pose_matrix()))
    except KeyError:
        print("Controller not available. Sure that the tracepen is turned on?")
        return None
    except TypeError:
        print("Tracepen not available")
        print("Is SteamVR running?")
        return None

    # Create 4x4 identity matrix
    tracepen_matrix = np.eye(4)

    # Set the translation vector in the matrix
    tracepen_matrix[:3, 3] = raw_tracepen_matrix[:3, 3]

    # Create z flip matrix to mirror along the z axis and change the frame handedness
    flip_z_matrix = np.diag([-1, -1, 1])

    # Extract rotation component of the raw matrix, apply z-flip and set in the tracepen matrix
    tracepen_matrix[:3, :3] = raw_tracepen_matrix[:3, :3] @ flip_z_matrix

    return tracepen_matrix

---

Many thanks for your help! I am so glad that this is finally solved.

---

PROGRESS. 90% THERE!

The problem is so stupid i am ashamed of sharing where it came from.

Additionally: is the triad_vr you are using a local customised version? Searching for getPoseQuaternion() only returns your two posts on SO and this ROS Answers post.

Yep. That is it. To my defense, i got the workspace from my supervisor and nerver questined that the base is faulty. But it is.

getPoseQuaternion() should not exist. The name alone is suspicious.

A pose has 6DOF. Translation AND rotation. A quaternion has 3DOF. The implementation was complete nonsense and also not necessary at all.

triadvr comes with getpose_matrix() which i want anyways for furrther calculation of the calibration matrix.

---

With this, things look much better.

I changed the socket orientation to support the tracepen as i want it. The tracepen axis are:

• z axial, positive towards the tip
• x is towards right
• y down (right handed)

Now i have the following behaviour:

• Moving the (real) pen in z direction moves it in z direction. Rotations around z are also as expected.
• Moving in +x (real) gives me -x in rviz. Also rotation around x is mirrored.
• Moving +y (real) gives me -y in rviz.Also rotation around y is mirrored.

---

Further, the tracepen origin calculation depends on the socket position, which should not be the case.

Also, it is near the world orign (front bottom left) when actually it should be sometwhere reat top right near the start of the panel holder arm(as the tracepenMatrix values for translation are 0 there.

First calibration pose. Everything looks ok:

---

Now with socket moved 0.2 in +x (tracepen still plugged in the socket) - the tracepen should also move +0.2x, but moves -0.2X.

---

New calibration in this pose.

The tracepen base frame changed its location. It should actually be independent of the calibration socket pose and remain constant.

The full, prettied code with all unnecessary stuff removed: https://github.com/Infraviored/ThesisStuff/blob/main/tracepen_node_v3

or also here:

#!/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 triad_openvr as triad_openvr

import tf.transformations
import rospkg
import os

import argparse


class TracepenNode(object):
    def __init__(self):
        parser = argparse.ArgumentParser()
        parser.add_argument("--record", action='store_true')
        parser.add_argument("interval", type=float, nargs='?',
                            default=250.0, help="Interval for TracepenNode")
        args = parser.parse_args()
        self.record = args.record

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

        self.interval = 1/args.interval

        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

        """
        tracepenMatrix = None
        while tracepenMatrix is None:
            tracepenMatrix = self.tracepenMatrix()
            rospy.sleep(0.1)

    def tracepenMatrix(self):
        """
        Returns the tracepen matrix as a homogeneous 4x4 transformation matrix.

        This method retrieves the pose matrix from the controller, converts it into
        a numpy array, and inserts it into the first three rows of a 4x4 identity matrix.
        The resulting matrix represents the pose of the tracepen in homogeneous coordinates.

        Returns:
            np.ndarray: A 4x4 homogeneous transformation matrix representing the pose of the tracepen.
        """
        mirrorXYMatrix = np.array([[-1, 0, 0, 0],
                                [0, -1, 0, 0],
                                [0, 0, 1, 0],
                                [0, 0, 0, 1]])
        tracepenMatrix = np.eye(4)
        tracepenMatrix[:3] = np.array(
            list(self.v.devices["controller_1"].get_pose_matrix()))
        mirroredTracepenMatrix = mirrorXYMatrix@tracepenMatrix

        return mirroredTracepenMatrix

    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.
        matrix_tracepenOrigin_tracepenPlugged = self.tracepenMatrix()
        tf_tracepenOrigin_tracepenPlugged = self.tf_from_matrix(
            matrix_tracepenOrigin_tracepenPlugged, "tracepenOrigin", "tracepenPlugged")

        matrix_tracepenPlugged_tracepenOrigin = np.linalg.inv(
            matrix_tracepenOrigin_tracepenPlugged)

        # 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(4)

        pkg_path = rospkg.RosPack().get_path('fmp_tracepen_node')
        # Create the 'data' directory inside the package directory (if it doesn't exist yet)
        data_dir = os.path.join(pkg_path, 'data')
        os.makedirs(data_dir, exist_ok=True)
        # Open the file for writing before the while loop starts. Use mode='a' to create the file if it doesn't exist yet.
        with open(os.path.join(data_dir, 'tracepenPose.txt'), "a") as f:
            while not rospy.is_shutdown():

                # Get data from OpenVr
                tracepenMatrix = self.tracepenMatrix()

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

                tf_tracepenOrigin_tracepen = self.tf_from_matrix(
                    tracepenMatrix, "tracepenOrigin", "tracepen")
                self.tf_broadcaster.sendTransform(tf_tracepenOrigin_tracepen)

                # Write to the file inside the while loop
                if self.record == True:
                    f.write("\n%s\n" % tf_tracepenOrigin_tracepen)
                print('\n\n %s \n\n' % 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

---

not important anymore

---

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: matrixworldtracepenPlugged = matrixworldsocket
• I get the total homogeneous transformation from world to tracepenOrigin as
• matrixworldtracepenOrigin = matrixworldtracepenPlugged@ matrixtracepenPluggedtracepenOrigin
• from this i can publish the tf(world,tracepenOrigin)
• as i have the tracepenPose() as tf(tracepenOrigin, tracepen), i can now publish tf(world,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 there remains unexpected bahaviour.

---

ERROR - TESTS

Reading out tracepenPose() directly and then moving it to find the 0 translation leads to a point on the right side up. I makred this with the Frame Demo Origin in the following images.

The calulcated tracepenOrigin is not near this point.

If everything works, the tracepenOrigin is found and should remain the same, independent of the Socket location. However when calulcating the tracepen Origin from 3 different socket locations, i get 3 different tracepen origin frames:

First socket locaton

---

Second socket location, shiftet by x+0.4m from first

---

Third socket location, rotated around x by 40 degrees from 2nd

---

I think maybe this could be explained by wrongly defining the tracepenPose() rotation components. And the current assignment

position.pose.orientation.x = tracepenpose[4] position.pose.orientation.y = tracepenpose[5] position.pose.orientation.z = tracepenpose[6] position.pose.orientation.w = tracepenpose[3]

also looks suspicious. I tried exchaning two indices, which completely cahnges the calculated tracepen Origin. But i also did not find an order in which everything looks fine.

Maybe the problem is here, maybe not.

---

Old tests

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

#!/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

---