xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改姿态问题

Browse Source
This commit is contained in:
cdeyw
2024-12-17 19:54:11 +08:00
parent bbecd2012c cc8f2e483c
commit 3857d69d23
2 changed files with 54 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
Trace/handeye_calibration.py
View File

@ -54,7 +54,7 @@ def R_matrix(x,y,z,u,v,w):
# 图像识别结果：xyz和法向量 # 图像识别结果：xyz和法向量
def getPosition(x,y,z,a,b,c,rotation,points): def getPosition(x,y,z,a,b,c,points):
target = np.asarray([x, y, z,1]) target = np.asarray([x, y, z,1])
camera2robot = np.loadtxt('./Trace/com_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep camera2robot = np.loadtxt('./Trace/com_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep
# robot2base = rotation # robot2base = rotation
@ -62,16 +62,22 @@ def getPosition(x,y,z,a,b,c,rotation,points):
target_position = np.dot(camera2robot, target) target_position = np.dot(camera2robot, target)
corner_points_camera = np.asarray(points) corner_points_camera = np.asarray(points)
corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
edges = np.array([corner_points_base[1] - corner_points_base[0]]) # for i in range(len(corner_points_base))]) # 按照 x 轴排序
edge_lengths = np.linalg.norm(edges, axis=1) sorted_points = corner_points_base[np.argsort(corner_points_base[:, 0])]
min_edge_idx = np.argmin(edge_lengths) # 选出x轴较大的两个点
short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量 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)
angle = np.asarray([a,b,c]) angle = np.asarray([a,b,c])
noraml = camera2robot[:3, :3]@angle noraml = camera2robot[:3, :3]@angle
noraml_base = vec2rpy(noraml,short_edge_direction) noraml_base = vec2rpy(noraml,short_edge_direction)
print("111",target_position, noraml_base)
return target_position,noraml_base return target_position,noraml_base

63
Trace/vec_change.py
View File

@ -35,36 +35,53 @@ T_BC = np.loadtxt('./com_pose.txt', delimiter=' ')
# T_AC = T_AB @ T_BC # T_AC = T_AB @ T_BC
# 输入四个角点的空间坐标 (相机坐标系下) # 输入四个角点的空间坐标 (相机坐标系下)
corner_points_camera = np.array([
[-804.54114, -259.56854, 1764.0],
[-466.9239, -266.63162, 1812.0],
[-451.46283, 206.7851, 1752.0],
[-762.1249, 197.22481, 1671.0]
])
# corner_points_camera = np.array([ # corner_points_camera = np.array([
# [-0.07246355234803807, 0.03687307395179221, 0.6171704935761987], # [-605.3829, 288.2771, 1710.0],
# [0.02505911529466708, 0.04860494901872052, 0.625793874394482], # [-364.94568, 300.40274, 1634.0],
# [0.03136683809654154, 0.003582437967995489, 0.6478950644491274], # [-301.4996, -253.04178, 1645.0],
# [-0.07010334103611927, -0.007624093814835717, 0.638128259308727] # [-548.8065, -297.23093, 1748.0]
# ])
# #
# ]) # # 将角点从相机坐标系转换到基坐标系
# corner_points_camera = np.array([ #
# [-0.07010334103611927, -0.007624093814835717, 0.638128259308727], # corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3]
# [0.03136683809654154, 0.003582437967995489, 0.6478950644491274], # edges = np.array([corner_points_base[1] - corner_points_base[0]])# for i in range(len(corner_points_base))])
# [0.02505911529466708, 0.04860494901872052, 0.625793874394482], # edge_lengths = np.linalg.norm(edges, axis=1)
# [-0.07246355234803807, 0.03687307395179221, 0.6171704935761987] # min_edge_idx = np.argmin(edge_lengths)
# ]) # short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量
# 将角点从相机坐标系转换到法兰坐标系
corner_points_camera = np.array([
[-548.8065, -297.23093, 1748.0],
[-301.4996, -253.04178, 1645.0],
[-364.94568, 300.40274, 1634.0],
[-605.3829, 288.2771, 1710.0]
])
# 将角点从相机坐标系转换到基坐标系
corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3]
edges = np.array([corner_points_base[1] - corner_points_base[0]])# for i in range(len(corner_points_base))])
edge_lengths = np.linalg.norm(edges, axis=1) # 按照 x 轴排序
min_edge_idx = np.argmin(edge_lengths) sorted_points = corner_points_base[np.argsort(corner_points_base[:, 0])]
short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量
# 选出x轴较大的两个点
point_1 = sorted_points[-1] # x值较大的点
point_2 = sorted_points[-2] # x值较小的点
# 根据 y 值选择差值方向，y值较大的点减去 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)
print("方向向量（单位化）：", short_edge_direction)
# 假设法向量 (a, b, c) 在相机坐标系下 # 假设法向量 (a, b, c) 在相机坐标系下
normal_vector_camera = np.array([-0.23022874142597569, 0.12287340685252988, 0.965348047343477, 0]) # 最后一个元素为0，因为它是方向矢量 normal_vector_camera = np.array([0.2694268969253701, 0.033645691818738714, 0.9624329143556991, 0]) # 最后一个元素为0，因为它是方向矢量
# 将法向量从相机坐标系转换到法兰坐标系 # 将法向量从相机坐标系转换到法兰坐标系
normal_vector_flange = T_BC @ normal_vector_camera normal_vector_flange = T_BC @ normal_vector_camera