更新 Trace/handeye_calibration

2024-08-27 06:14:46 +00:00
parent 2327a19e25
commit bb8fbfe8ad
1 changed files with 2 additions and 1 deletions
--- a/Trace/handeye_calibration
+++ b/Trace/handeye_calibration
@ -45,7 +45,8 @@ def getPosition(x,y,z,a,b,c):
    # angle_tool = R.from_matrix(camera2robot[0:3, 0:3])
    rpy = vec2attitude(a,b,c)
    r,p,y=rpy[0],rpy[1],rpy[2]#r表示u,p表示v，y表示w
-    target_angle = np.asarray([r,p,y])
+    target_angle_raw = np.asarray([r,p,y])
+    target_angle = np.dot(camera2robot[0:3, 0:3],target_angle_raw)
    print(target_position,target_angle)

    return target_position, target_angle