修正沿法向量方向运动问题

Trace/handeye_calibration.py

@@ -54,12 +54,12 @@ 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_pose2.txt', delimiter=' ') #相对目录且分隔符采用os.sep
     # robot2base = rotation
     # camera2base = robot2base @ camera2robot
-    target_position = np.dot(camera2robot, target)
+    target_position_raw = np.dot(camera2robot, target)
 
     corner_points_camera = np.asarray(points)
     corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
@@ -76,8 +76,14 @@ def getPosition(x,y,z,a,b,c,rotation,points):
     # 单位化方向向量
     short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
 
+    delta = -30#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
     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 沿法向量移动
+
     noraml_base = vec2rpy(noraml,short_edge_direction)
 
     return target_position,noraml_base
+
+