更新 Trace/handeye_calibration.py

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