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]
发布者:admin,转转请注明出处:http://www.yc00.com/questions/1744332663a4568963.html
评论列表(0条)