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:
hjw
2025-02-26 14:34:22 +00:00
parent ef50aed6bb
commit 6a169bd915
1 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
Trace/handeye_calibration.py
View File

@ -76,7 +76,7 @@ def getPosition(x,y,z,a,b,c,rotation,points):
# 单位化方向向量 # 单位化方向向量
short_edge_direction = edge_vector / np.linalg.norm(edge_vector) short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
delta = -200#沿法向量方向抬高和压低，-指表示抬高，+值表示压低 delta = 0#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
angle = np.asarray([a,b,c]) angle = np.asarray([a,b,c])
noraml = camera2robot[:3, :3]@angle noraml = camera2robot[:3, :3]@angle
normal_vector = noraml / np.linalg.norm(noraml) normal_vector = noraml / np.linalg.norm(noraml)
@ -86,4 +86,14 @@ def getPosition(x,y,z,a,b,c,rotation,points):
return target_position,noraml_base return target_position,noraml_base
def getxyz(x,y,z,a,b,c):
target = np.asarray([x, y, z, 1])
camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ')
target_position_raw = np.dot(camera2robot, target)
delta = -200 # 沿法向量方向抬高和压低，-指表示抬高，+值表示压低
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
return target_position