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

更新 Trace/handeye_calibration.py

Browse Source
This commit is contained in:
cdeyw
2024-12-09 06:26:16 +00:00
parent e43859b522
commit 44ff83a7d5
1 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
Trace/handeye_calibration.py
View File

@ -1,10 +1,10 @@
import numpy as np
from scipy.spatial.transform import Rotation as R
def vec2rpy(normal,long_edge_direction):
def vec2rpy(normal,short_edge_direction):
# 将法向量的反方向作为机械臂末端执行器的新Z轴
z_axis = normal / np.linalg.norm(normal) # 归一化并取反向作为Z轴
x_axis = long_edge_direction/np.linalg.norm(long_edge_direction)
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)
@ -57,21 +57,21 @@ def R_matrix(x,y,z,u,v,w):
def getPosition(x,y,z,a,b,c,rotation,points):
target = np.asarray([x, y, z,1])
camera2robot = np.loadtxt('./Trace/com_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep
robot2base = rotation
camera2base = robot2base @ camera2robot
target_position = np.dot(camera2base, target)
# robot2base = rotation
# camera2base = robot2base @ camera2robot
target_position = np.dot(camera2robot, target)
corner_points_camera = np.asarray(points)
corner_points_base = np.dot(camera2base[:3, :3], corner_points_camera.T).T + camera2base[:3, 3]
corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
edges = np.array([corner_points_base[i] - corner_points_base[i - 1] for i in range(len(corner_points_base))])
edge_lengths = np.linalg.norm(edges, axis=1)
max_edge_idx = np.argmax(edge_lengths)
long_edge_direction = edges[max_edge_idx] / edge_lengths[max_edge_idx] # 单位化方向向量
min_edge_idx = np.argmin(edge_lengths)
short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量
angle = np.asarray([a,b,c])
noraml = camera2base[:3, :3]@angle
noraml_base = vec2rpy(noraml,long_edge_direction)
noraml = camera2robot[:3, :3]@angle
noraml_base = vec2rpy(noraml,short_edge_direction)
print(target_position, noraml_base)
print("111",target_position, noraml_base)
return target_position,noraml_base