Merge remote-tracking branch 'origin/master'

Trace/com_pose.txt

@@ -1,4 +1,4 @@
--0.000714604 0.999379 -0.0352319 -19.4992
--0.992053 -0.00514141 -0.125718 28.7299
--0.125821 0.0348621 0.99144 -107.48
+9.4566884811714796e-02 -9.9470945966114444e-01 4.0127725032944608e-02 4.1471010091895931e+02
+-9.9551731828304890e-01 -9.4428128820258375e-02 5.3435988155243787e-03 1.9335993881936060e+03
+-1.5261424109928572e-03 -4.0453173929085366e-02 -9.9918028231904499e-01 2.7052051690106582e+03
 0 0 0 1

Trace/handeye_calibration.py

@@ -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