Transformation between two coordinate frames (tf)

edit

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.

triad_vr comes with get_pose_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 ...