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和法向量
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])
camera2robot = np.loadtxt('./Trace/com_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep
# robot2base = rotation
@ -62,16 +62,22 @@ def getPosition(x,y,z,a,b,c,rotation,points):
target_position = np.dot(camera2robot, target)
corner_points_camera = np.asarray(points)
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)
min_edge_idx = np.argmin(edge_lengths)
short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量
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)
angle = np.asarray([a,b,c])
noraml = camera2robot[:3, :3]@angle
noraml_base = vec2rpy(noraml,short_edge_direction)
print("111",target_position, noraml_base)
return target_position,noraml_base