diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index b2f5626..4e2d7d9 100644 --- a/Trace/handeye_calibration.py +++ b/Trace/handeye_calibration.py @@ -3,7 +3,7 @@ from scipy.spatial.transform import Rotation as R def vec2rpy(normal,long_edge_direction): # 将法向量的反方向作为机械臂末端执行器的新Z轴 - z_axis = -normal / np.linalg.norm(normal) # 归一化并取反向作为Z轴 + z_axis = normal / np.linalg.norm(normal) # 归一化并取反向作为Z轴 x_axis = long_edge_direction/np.linalg.norm(long_edge_direction) x_axis = x_axis-np.dot(x_axis,z_axis)*z_axis