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
2025-03-04 15:52:54 +08:00
parent a9cce7ec3a
commit 69e88c2b6b
9 changed files with 7230 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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