Find a Point by using Transformation Matrix and Inverse Tranformation Matrix

I have prepared simple code. I need to verify firstframe_center_point by using inverse second_frame_matrix and secondframe_center_point,however, there is a small mistake in my way. I have simply created my transformation function which is 4x4 matrix. I am giving 6 DOF values.

The matrices have been applied sequentially to the point starting from the origin (point [0, 0, 0]), meaning that each transformation builds upon the last.

import matplotlib.pyplot as plt
from matplotlib.backends.backend_tkagg import *
import tkinter as tk  
import numpy as np
import math

def transformation_matrix(trans_X = 0, trans_Y = 0, trans_Z = 0, rot_X = 0, rot_Y = 0, rot_Z = 0, ord='ZYX'):
    def rotation_matrix(X_deg=0,Y_deg=0,Z_deg=0, order='ZYX'):
        X_deg = math.radians(X_deg)
        Y_deg = math.radians(Y_deg)
        Z_deg = math.radians(Z_deg)

        R_x = np.array([[1, 0, 0],
                        [0, np.cos(X_deg), -np.sin(X_deg)],
                        [0, np.sin(X_deg), np.cos(X_deg)]])

        R_y = np.array([[np.cos(Y_deg), 0, np.sin(Y_deg)],
                        [0, 1, 0],
                        [-np.sin(Y_deg), 0, np.cos(Y_deg)]])
                
        R_z = np.array([[np.cos(Z_deg), -np.sin(Z_deg),0],
                        [np.sin(Z_deg), np.cos(Z_deg),0],
                        [0, 0, 1]])
        
        # Combine rotations in the specified sequence
        if order == 'ZYX':
            R = R_z @ R_y @ R_x
        elif order == 'XYZ':
            R = R_x @ R_y @ R_z
        elif order == 'YXZ':
            R = R_y @ R_x @ R_z
        else:
            raise ValueError("Unsupported rotation sequence")

        return R
    
    rot_matrix_3x3 = rotation_matrix(X_deg=rot_X,Y_deg=rot_Y,Z_deg=rot_Z,order=ord)
    trans_matrix_4x4 = np.array([[0.0, 0.0, 0.0, 0.0],
                                 [0, 0, 0, 0],
                                 [0, 0, 0, 0],
                                 [0, 0, 0, 1]])
    
    trans_matrix_4x4[:3, :3] = rot_matrix_3x3
    trans_matrix_4x4[0,3] = trans_X
    trans_matrix_4x4[1,3] = trans_Y
    trans_matrix_4x4[2,3] = trans_Z
    return trans_matrix_4x4

# Setup the plot
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
#################################################################################################
# World Frame (Global)
world_origin = np.array([[0.0], [0.0], [0.0], [1.0]])

# X, Y, Z axes of the world frame (global)
X_world = np.array([[10], [0.0], [0.0], [1.0]])
Y_world = np.array([[0.0], [10], [0.0], [1.0]])
Z_world = np.array([[0.0], [0.0], [10], [1.0]])

# Plot the world frame axes
ax.plot([world_origin[0], X_world[0]], [world_origin[1], X_world[1]], [world_origin[2], X_world[2]], linewidth=2, color='red')
ax.plot([world_origin[0], Y_world[0]], [world_origin[1], Y_world[1]], [world_origin[2], Y_world[2]], linewidth=2, color='green')
ax.plot([world_origin[0], Z_world[0]], [world_origin[1], Z_world[1]], [world_origin[2], Z_world[2]], linewidth=2, color='blue')

# Add text for the world frame
ax.text(world_origin[0][0] + 5, world_origin[1][0] + 5, world_origin[2][0] + 5, "World_Frame", fontsize=15, color="black")

############################################################################################################
# First Frame
first_frame_matrix = transformation_matrix(trans_X = 10, trans_Y = 20, trans_Z = 30, rot_X = 30, rot_Y = 60, rot_Z = 90)
firstframe_center_point = first_frame_matrix @ np.array([[0.0], [0.0], [0.0], [1.0]])

# X, Y, Z axes of the first frame
X_line = first_frame_matrix @ np.array([[10], [0.0], [0.0], [1.0]])
Y_line = first_frame_matrix @ np.array([[0.0], [10], [0.0], [1.0]])
Z_line = first_frame_matrix @ np.array([[0.0], [0.0], [10], [1.0]])

# Plot the first frame axes
ax.plot([firstframe_center_point[0], X_line[0]], [firstframe_center_point[1], X_line[1]], [firstframe_center_point[2], X_line[2]], linewidth=2, color='red')
ax.plot([firstframe_center_point[0], Y_line[0]], [firstframe_center_point[1], Y_line[1]], [firstframe_center_point[2], Y_line[2]], linewidth=2, color='green')
ax.plot([firstframe_center_point[0], Z_line[0]], [firstframe_center_point[1], Z_line[1]], [firstframe_center_point[2], Z_line[2]], linewidth=2, color='blue')

# Add text for the first frame
ax.text(firstframe_center_point[0][0] + 5, firstframe_center_point[1][0] + 5, firstframe_center_point[2][0] + 5, "First_Frame", fontsize=15)

############################################################################################################
# Second Frame
second_frame_matrix = transformation_matrix(trans_X = 20, trans_Y = 10, trans_Z = 15, rot_X = 60, rot_Y = 90, rot_Z = 120)
secondframe_center_point = first_frame_matrix @ second_frame_matrix @ np.array([[0.0], [0.0], [0.0], [1.0]])

# X, Y, Z axes of the second frame in global coordinates
X_line2 = first_frame_matrix @ second_frame_matrix @ np.array([[10], [0.0], [0.0], [1.0]])
Y_line2 = first_frame_matrix @ second_frame_matrix @ np.array([[0.0], [10], [0.0], [1.0]])
Z_line2 = first_frame_matrix @ second_frame_matrix @ np.array([[0.0], [0.0], [10], [1.0]])

# Plot the second frame axes
ax.plot([secondframe_center_point[0], X_line2[0]], [secondframe_center_point[1], X_line2[1]], [secondframe_center_point[2], X_line2[2]], linewidth=2, color='red')
ax.plot([secondframe_center_point[0], Y_line2[0]], [secondframe_center_point[1], Y_line2[1]], [secondframe_center_point[2], Y_line2[2]], linewidth=2, color='green')
ax.plot([secondframe_center_point[0], Z_line2[0]], [secondframe_center_point[1], Z_line2[1]], [secondframe_center_point[2], Z_line2[2]], linewidth=2, color='blue')

# Add text for the second frame
ax.text(secondframe_center_point[0][0] + 5, secondframe_center_point[1][0] + 5, secondframe_center_point[2][0] + 5, "Second_Frame", fontsize=15)

############################################################################################################
# First Frame Point Verification
firstframe_center_point_verify = np.linalg.inv(second_frame_matrix) @ secondframe_center_point

# Plot the verified first frame center point in green
ax.scatter(firstframe_center_point_verify[0], firstframe_center_point_verify[1], firstframe_center_point_verify[2], color="green", marker='v', s=200)
ax.text(firstframe_center_point_verify[0][0]- 5, firstframe_center_point_verify[1][0] - 5, firstframe_center_point_verify[2][0] - 5, "firstframe_center_point_verify", fontsize=15)
print(first_frame_matrix @ second_frame_matrix @ np.linalg.inv(second_frame_matrix))
# Print the results
print("firstframe_center_point", firstframe_center_point)
print("firstframe_center_point_verify", firstframe_center_point_verify)
print("Are they the same?", np.allclose(firstframe_center_point, firstframe_center_point_verify))

# Show the plot
plt.axis('equal')
plt.show()

[][1tp]