first commit

Trace/handeye_calibration.py

@@ -0,0 +1,104 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+def vec2rpy(normal,short_edge_direction):
+    # 将法向量的反方向作为机械臂末端执行器的新Z轴
+    z_axis = (-normal / np.linalg.norm(normal))  # 归一化并取反向作为Z轴
+    x_axis = short_edge_direction/np.linalg.norm(short_edge_direction)
+
+    x_axis = x_axis-np.dot(x_axis,z_axis)*z_axis
+    x_axis = x_axis/np.linalg.norm(x_axis)
+
+    y_axis = np.cross(z_axis,x_axis)
+
+    # 构造旋转矩阵
+    rotation_matrix = np.vstack([x_axis, y_axis, z_axis]).T
+
+    # 将旋转矩阵转换为RPY（roll, pitch, yaw）
+    rpy = R.from_matrix(rotation_matrix).as_euler('xyz', degrees=True)
+
+    return rpy
+
+#张啸给我的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)]
+    ])
+
+    R_y = np.array([
+        [np.cos(ry), 0, np.sin(ry)],
+        [0, 1, 0],
+        [-np.sin(ry), 0, np.cos(ry)]
+    ])
+
+    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])
+
+    # 构建齐次变换矩阵
+    transformation_matrix = np.eye(4)
+    transformation_matrix[:3, :3] = R
+    transformation_matrix[:3, 3] = T
+
+    return transformation_matrix
+
+
+# 图像识别结果：xyz和法向量
+def getPosition(x,y,z,a,b,c,rotation,points):
+    target = np.asarray([x, y, z,1])
+    camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ') #相对目录且分隔符采用os.sep
+    # robot2base = rotation
+    # camera2base = robot2base @ camera2robot
+    target_position_raw = np.dot(camera2robot, target)
+
+    corner_points_camera = np.asarray(points)
+    corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
+    # 按照 x 轴排序
+    sorted_points = corner_points_base[np.argsort(corner_points_base[:, 0])]
+    # 选出x轴较大的两个点
+    point_1 = sorted_points[-1]  # x值较大的点
+    point_2 = sorted_points[-2]  # x值较小的点
+    # 根据 y 值选择差值方向
+    if point_1[1] < point_2[1]:
+        edge_vector = point_1 - point_2
+    else:
+        edge_vector = point_2 - point_1
+    # 单位化方向向量
+    short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
+
+    delta = -10#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
+    angle = np.asarray([a,b,c])
+    noraml = camera2robot[:3, :3]@angle
+    normal_vector = noraml / np.linalg.norm(noraml)
+    target_position = target_position_raw[:3] + delta * normal_vector  # target_position 沿法向量移动
+
+    noraml_base = vec2rpy(noraml,short_edge_direction)
+
+    return target_position,noraml_base
+
+def getxyz(x,y,z,a,b,c):
+    target = np.asarray([x, y, z])
+    camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ')
+    # target_position_raw = np.dot(camera2robot, target)
+    delta = -500  # 沿法向量方向抬高和压低，-指表示抬高，+值表示压低
+    angle = np.asarray([a, b, c])
+    noraml = camera2robot[:3, :3] @ angle
+    normal_vector = noraml / np.linalg.norm(noraml)
+    target_position = target + delta * normal_vector
+
+    return target_position
+def getxyz1(x, y, z, a, b, c):
+    """
+    直接返回原始的 x, y, z 坐标，不做任何变换和偏移
+    """
+    return x, y, z, a, b, c