Inverse Transformation Matrix

[ozgur-kurt]

Hi, I am sorry if my question is off topic for your repo.

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

[AlexanderFabisch]

I converted the issue to a discussion, but I do not see how this is related to the repository at all. You don't use pytransform3d, but the library and the user guide would give you a lot of tools to solve your problem. As an example, the conversion from Euler angles to rotation matrices that you implemented is available in pytransform3d. The plotting feature that you implemented is available in pytransform3d. The user guide gives you some tips on how to verify that your sequence of transformations is correct. The TransformManager is a tool that you can use if you don't even want to think about how you have to combine your transformations and which one you have to invert and it also comes with plotting functions. I'd suggest to try that. I think your problem really demonstrates well why I wrote this library. 😄