diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index 65f6c2f..e9812fd 100644 --- a/Trace/handeye_calibration.py +++ b/Trace/handeye_calibration.py @@ -3,21 +3,21 @@ import os import numpy as np from scipy.spatial.transform import Rotation as R -def vec2rpy(normal):#首先是相机到法兰的转换,之后是法兰到新坐标系的转换(新坐标系就是与Z轴与法向量一致的坐标系) - # 将法向量作为机械臂末端执行器的新X轴 - x_axis = normal / np.linalg.norm(normal) # 归一化 +def vec2rpy(normal): + # 将法向量的反方向作为机械臂末端执行器的新Z轴 + z_axis = -normal / np.linalg.norm(normal) # 归一化并取反向作为Z轴 - # 选择一个合适的Y轴方向,尽量避免与X轴共线 - if abs(x_axis[2]) != 1: - y_axis = np.array([0, 1, 0]) + # 尝试选择一个合适的X轴方向,尽量避免与Z轴共线 + if abs(z_axis[2]) != 1: + x_axis = np.array([1, 0, 0]) else: - y_axis = np.array([0, 0, 1]) + x_axis = np.array([0, 1, 0]) - y_axis = y_axis - np.dot(y_axis, x_axis) * x_axis # 投影到垂直于x轴的平面 - y_axis = y_axis / np.linalg.norm(y_axis) + x_axis = x_axis - np.dot(x_axis, z_axis) * z_axis # 投影到垂直于Z轴的平面 + x_axis = x_axis / np.linalg.norm(x_axis) - # 计算Z轴方向,确保它与X轴和Y轴正交 - z_axis = np.cross(x_axis, y_axis) + # 计算Y轴方向,确保它与X轴和Z轴正交 + y_axis = np.cross(z_axis, x_axis) # 构造旋转矩阵 rotation_matrix = np.vstack([x_axis, y_axis, z_axis]).T