更新 Trace/handeye_calibration

Trace/handeye_calibration

@@ -1,6 +1,6 @@
 import numpy as np
-from scipy.spatial.transform import Rotation as R
 
+rotation = R_matrix()#张啸给我的值填这里
 
 def flip_coefficient_if_positive(coefficient):
     # 检查 coefficient[2] 是否大于0
@@ -11,7 +11,8 @@ def flip_coefficient_if_positive(coefficient):
 
     return coefficient
 
-def vec2ola(coefficient):
+def vec2ola(coefficient):#首先是相机到法兰的转换，之后是法兰到新坐标系的转换（新坐标系就是与Z轴与法向量一致的坐标系)
+    #如果不转换坐标轴，
     coefficient_raw = flip_coefficient_if_positive(coefficient)
     coefficient = coefficient_raw.reshape(-1)
     curZ = np.array(coefficient)# 定义 Z 方向的向量
@@ -34,65 +35,63 @@ def vec2ola(coefficient):
     ])
 
     # 打印旋转矩阵
-    print("Rotation Matrix:")
-    print(rotM)
+    # print("Rotation Matrix:")
+    # print(rotM)
 
     # 计算欧拉角（XYZ顺序）并转换为度
     r = R.from_matrix(rotM)
-    euler_angles = r.as_euler('xyz', degrees=True)
+    euler_angles = r.as_euler('xyz', degrees=True)#或者是zxy
 
-    print("Euler Angles (degrees):")
-    print(euler_angles)
+    # print("Euler Angles (degrees):")
+    # print(euler_angles)
+    return euler_angles
 
+#张啸给我的xyzuvw
+def R_matrix(x,y,z,u,v,w):
+    rx = np.radians(u)
+    ry = np.radians(v)
+    rz = np.radians(w)
+    # 定义绕 X, Y, Z 轴的旋转矩阵
+    R_x = np.array([
+        [1, 0, 0],
+        [0, np.cos(rx), -np.sin(rx)],
+        [0, np.sin(rx), np.cos(rx)]
+    ])
 
-
-def vec2attitude(a,b,c):
-    pi = np.arccos(-1.0)  # pi 的值
-
-    # 输入a, b, c
-    # 定义矩阵
-    r1 = np.array([[1, 0, 0],
-                   [0, np.cos(a * pi), np.sin(a * pi)],
-                   [0, -np.sin(a * pi), np.cos(a * pi)]])  # r1 矩阵 3x3
-
-    r2 = np.array([[np.cos(b * pi), 0, -np.sin(b * pi)],
+    R_y = np.array([
+        [np.cos(ry), 0, np.sin(ry)],
         [0, 1, 0],
-                   [np.sin(b * pi), 0, np.cos(b * pi)]])  # r2 矩阵 3x3
+        [-np.sin(ry), 0, np.cos(ry)]
+    ])
 
-    r3 = np.array([[np.cos(c * pi), np.sin(c * pi), 0],
-                   [-np.sin(c * pi), np.cos(c * pi), 0],
-                   [0, 0, 1]])  # r3 矩阵 3x3
+    R_z = np.array([
+        [np.cos(rz), -np.sin(rz), 0],
+        [np.sin(rz), np.cos(rz), 0],
+        [0, 0, 1]
+    ])
+    R = R_z @ R_y @ R_x
+    T = np.array([x, y, z])
 
-    vector1 = np.array([[1], [1], [1]])  # 初始向量 3x1
+    # 构建齐次变换矩阵
+    transformation_matrix = np.eye(4)
+    transformation_matrix[:3, :3] = R
+    transformation_matrix[:3, 3] = T
 
-    # 旋转矩阵相乘并应用于向量
-    matrix_result = np.dot(np.dot(np.dot(r1, r2), r3), vector1)
+    return transformation_matrix
 
-    # 输出结果
-    ola=[]
-    for m in range(matrix_result.shape[0]):
-        ola.append(matrix_result[m][0])
-        print(f"{matrix_result[m][0]:<20}")
 
-    return ola
-
-#这个版本是先拿法向量转换成基坐标，在转欧拉
-#黄老师会给我传目标物的中心点坐标x,y,z和目标位姿的平面法向量a,b,c
+# 黄老师给我的xyz和法向量
 def getPosition(x,y,z,a,b,c):
-    target = np.asarray([x, y, z])
+    target = np.asarray([x, y, z,1])
     camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
-    position = np.dot(camera2robot[0:3, 0:3], target) + camera2robot[0:3, 3:]
-    target_position = position[0:3, 0]#转换后的位置信息
+    robot2base = rotation
+    camera2base = robot2base @ camera2robot
+    target_position = np.dot(camera2base, target)
 
-    vector = np.asarray([a, b, c])
-    normal_vector = vector / np.linalg.norm(vector)#归一化
-    normal_vector.shape = (3, 1)
-    dot_angle = np.dot(camera2robot[0:3, 0:3], normal_vector)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
-
-    target_angle = vec2ola(dot_angle)#把转换之后的法向量转换为欧拉角,欧拉角不是rpy角
-
-    # r,p,y = angle_tool.as_euler('xyz',degrees=True)#r表示u,p表示v，y表示w
-    # target_angle = np.asarray([r,p,y])
-    # print(target_angle)
+    rpy = vec2ola(a, b, c)
+    r, p, y = rpy[0], rpy[1], rpy[2]  # r表示u,p表示v，y表示w
+    target_angle_raw = np.asarray([r, p, y])
+    target_angle = np.dot(camera2robot[0:3, 0:3], target_angle_raw)
+    print(target_position, target_angle)
 
     return target_position,target_angle