From 2327a19e25aff5c5b27bd4988a83c516f2140137 Mon Sep 17 00:00:00 2001
From: cdeyw <827523911@qq.com>
Date: Tue, 27 Aug 2024 05:58:04 +0000
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20Trace/handeye=5Fcalibratio?=
 =?UTF-8?q?n?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Trace/handeye_calibration | 46 ++++++++++++++++++++++++++++++++-------
 1 file changed, 38 insertions(+), 8 deletions(-)

diff --git a/Trace/handeye_calibration b/Trace/handeye_calibration
index dbe0a03..ae5f7be 100644
--- a/Trace/handeye_calibration
+++ b/Trace/handeye_calibration
@@ -1,6 +1,34 @@
 import numpy as np
-from scipy.spatial.transform import Rotation as R
 
+def vec2attitude(a,b,c):
+    pi = np.arccos(-1.0)  # pi 的值
+
+    # 输入a, b, c
+    # 定义矩阵
+    r1 = np.array([[1, 0, 0],
+                   [0, np.cos(a * pi), np.sin(a * pi)],
+                   [0, -np.sin(a * pi), np.cos(a * pi)]])  # r1 矩阵 3x3
+
+    r2 = np.array([[np.cos(b * pi), 0, -np.sin(b * pi)],
+                   [0, 1, 0],
+                   [np.sin(b * pi), 0, np.cos(b * pi)]])  # r2 矩阵 3x3
+
+    r3 = np.array([[np.cos(c * pi), np.sin(c * pi), 0],
+                   [-np.sin(c * pi), np.cos(c * pi), 0],
+                   [0, 0, 1]])  # r3 矩阵 3x3
+
+    vector1 = np.array([[1], [1], [1]])  # 初始向量 3x1
+
+    # 旋转矩阵相乘并应用于向量
+    matrix_result = np.dot(np.dot(np.dot(r1, r2), r3), vector1)
+
+    # 输出结果
+    rpy=[]
+    for m in range(matrix_result.shape[0]):
+        rpy.append(matrix_result[m][0])
+        print(f"{matrix_result[m][0]:<20}")
+
+    return rpy
 
 #黄老师会给我传目标物的中心点坐标x,y,z和目标位姿的平面法向量a,b,c
 def getPosition(x,y,z,a,b,c):
@@ -9,13 +37,15 @@ def getPosition(x,y,z,a,b,c):
     position = np.dot(camera2robot[0:3, 0:3], target) + camera2robot[0:3, 3:]
     target_position = position[0:3, 0]#转换后的位置信息
 
-    vector = np.asarray([a, b, c])
-    normal_vector = vector / np.linalg.norm(vector)#归一化
-    normal_vector.shape = (3, 1)
-    dot_angle = np.dot(camera2robot[0:3, 0:3], normal_vector)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
+    # vector = np.asarray([a, b, c])
+    # normal_vector = vector / np.linalg.norm(vector)#归一化
+    # normal_vector.shape = (3, 1)
+    # dot_angle = np.dot(camera2robot[0:3, 0:3], normal_vector)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
 
-    angle_tool = R.from_matrix(camera2robot[0:3, 0:3])
-    r,p,y = angle_tool.as_euler('xyz',degrees=True)#r表示u,p表示v，y表示w
+    # 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])
+    print(target_position,target_angle)
 
-    return target_position, dot_angle, target_angle
+    return target_position, target_angle