I have given following transformation:

arguments=['-0.005', '0.1370', '0.00548', '0.0148379', '-0.1494206', '0.9886599', '-0.0021957', 'wrist_3_link', 'camera_color_optical_frame']

This is being published by static_transform_publihser.

Next, from tf_tree I can get following transformation: rosrun tf tf_echo camera_bottom_screw camera_color_optical_frame which prints:

translation: [0.008, 0.000, 0.045] 
rotation: in quaternion [-0.5, 0.5, -0.5, 0.5]
in rpy (radians) [-1.571, -0.000, -1.571]

I want to obtain transformation from wrist_3_link to camera_bottom_screw. I want this cuz I assume these two transformations above are ground truth (without error). Because first one is obtained from calibration (let's assume it's perfect) and second one is from URDF of realsense camera, provided by manufacturer. Previously transformation between wrist_3_link and camera_bottom_screw is defined by someone, and it is not correct. Now since I have these two transformations, I can obtain correct transformation between wrist_3_link and camera_bottom_screw.

Let's call these frames like this:

A - wrist_3_link
B - camera_bottom_screw
C - camera_color_optical_frame

But I have issue with (I guess) ROS conventions. I tried so many times but cannot get it right. When I first publish transfrom T_C_A frame C is placed with respect to frame A. Now I want to apply T_B_A and T_C_B to obtain same transformation i.e. same placement of frame C wrt to A.

T_C_A = T_C_B * T_B_A

This is my code for calculation T_C_B:

import numpy as np
from scipy.spatial.transform import Rotation as R

def compute_transformation(T_C_A, T_C_B):
    """
    Compute the transformation T_B_A (from A to B) given T_C_A and T_C_B.
    
    Args:
        T_C_A: Transformation from A to C as (tx, ty, tz, qx, qy, qz, qw)
        T_C_B: Transformation from B to C as (tx, ty, tz, qx, qy, qz, qw)
        
    Returns:
        T_B_A: Transformation from A to B as (tx, ty, tz, qx, qy, qz, qw)
    """
    # Unpack T_C_A and T_C_B
    t_C_A, q_C_A = T_C_A[:3], T_C_A[3:]
    t_C_B, q_C_B = T_C_B[:3], T_C_B[3:]
    
    # Create 4x4 transformation matrices
    T_C_A_matrix = np.eye(4)
    T_C_A_matrix[:3, :3] = R.from_quat(q_C_A).as_matrix()
    T_C_A_matrix[:3, 3] = t_C_A

    T_C_B_matrix = np.eye(4)
    T_C_B_matrix[:3, :3] = R.from_quat(q_C_B).as_matrix()
    T_C_B_matrix[:3, 3] = t_C_B

    # Compute T_B_A = T_C_B^-1 * T_C_A
    T_B_C_matrix = np.linalg.inv(T_C_B_matrix)
    T_B_A_matrix = T_B_C_matrix @ T_C_A_matrix

    # Extract translation and rotation from T_B_A_matrix
    t_B_A = T_B_A_matrix[:3, 3]
    q_B_A = R.from_matrix(T_B_A_matrix[:3, :3]).as_quat()
    q_B_A /= np.linalg.norm(q_B_A)  # Ensure it's normalized
    rpy_B_A = R.from_matrix(T_B_A_matrix[:3, :3]).as_euler('xyz', degrees=False) # better q (no ambiguity)

    return T_B_A_matrix, t_B_A, q_B_A, rpy_B_A, T_C_A_matrix, T_C_B_matrix

# Example input: T_C_A and T_C_B
T_C_A = [-0.005, 0.1370, 0.00548, 0.0148379, -0.1494206, 0.9886599, -0.0021957]
T_C_B = [0.008, 0.0, 0.045, -0.5, 0.5, -0.5, 0.5]  # Translation + quaternion

# Compute T_B_A
T_B_A_matrix, t_B_A, q_B_A, rpy_B_A, T_C_A_matrix, T_C_B_matrix = compute_transformation(T_C_A, T_C_B)

T_test = T_C_B_matrix @ T_B_A_matrix
print("T_test")
print(T_test) # Same as T_C_A (I guess everything fine now)

But when I publish T_C_B: rosrun tf static_transform_publisher 0.008 0.0 0.045 -0.5 0.5 -0.5 0.5 B C 10

and T_B_A (obtained by my code): rosrun tf static_transform_publisher -0.137 0.03952 -0.013 0.41329857 0.56052348 -0.42594077 0.57755708 A B 10

I am not getting same placement of frame C with respect to A. I tried so many times, but cannot solve it. I think there are some misunderstandings in axes orientations and similar, which I cannot solve.

These are results (first one aplying T_C_A) and second one these two consecutive transforms:

faris@faris-lenovo:~$ rosrun tf tf_echo wrist optical
At time 1737282624.483
- Translation: [-0.005, 0.137, 0.005]
- Rotation: in Quaternion [0.015, -0.149, 0.989, -0.002]
            in RPY (radian) [-0.300, -0.029, -3.133]
            in RPY (degree) [-17.196, -1.644, -179.497]
^Cfaris@faris-lenovo:~$ rosrun rqt_tf_tree rqt_tf_tree 
faris@faris-lenovo:~$ rosrun rqt_tf_tree rqt_tf_tree 
faris@faris-lenovo:~$ rosrun rqt_tf_tree rqt_tf_tree 
faris@faris-lenovo:~$ rosrun tf tf_echo wrist optical
At time 1737282746.657
- Translation: [-0.124, -0.004, -0.020]
- Rotation: in Quaternion [-0.149, 0.989, -0.015, 0.002]
            in RPY (radian) [-3.112, -0.000, -2.842]
            in RPY (degree) [-178.281, -0.005, -162.811]