98 lines
3.2 KiB
Plaintext
98 lines
3.2 KiB
Plaintext
import numpy as np
|
||
from scipy.spatial.transform import Rotation as R
|
||
|
||
|
||
def flip_coefficient_if_positive(coefficient):
|
||
# 检查 coefficient[2] 是否大于0
|
||
if coefficient[2] > 0:
|
||
# 取反所有分量
|
||
coefficient = [-x for x in coefficient]
|
||
print("翻转：\n")
|
||
|
||
return coefficient
|
||
|
||
def vec2ola(coefficient):
|
||
coefficient_raw = flip_coefficient_if_positive(coefficient)
|
||
coefficient = coefficient_raw.reshape(-1)
|
||
curZ = np.array(coefficient)# 定义 Z 方向的向量
|
||
# print(curZ)
|
||
|
||
curX = np.array([1, 1, 0],dtype=np.float64)# 定义初始 X 方向的向量
|
||
curX /= np.linalg.norm(curX) # 归一化 X 向量
|
||
|
||
curY = np.cross(curZ, curX)# 计算 Y 方向的向量
|
||
curY /= np.linalg.norm(curY) # 归一化 Y 向量
|
||
|
||
curX = np.cross(curY, curZ)# 重新计算 X 方向的向量
|
||
curX /= np.linalg.norm(curX) # 归一化 X 向量
|
||
|
||
# 创建旋转矩阵
|
||
rotM = np.array([
|
||
[curX[0], curY[0], curZ[0]],
|
||
[curX[1], curY[1], curZ[1]],
|
||
[curX[2], curY[2], curZ[2]]
|
||
])
|
||
|
||
# 打印旋转矩阵
|
||
print("Rotation Matrix:")
|
||
print(rotM)
|
||
|
||
# 计算欧拉角（XYZ顺序）并转换为度
|
||
r = R.from_matrix(rotM)
|
||
euler_angles = r.as_euler('xyz', degrees=True)
|
||
|
||
print("Euler Angles (degrees):")
|
||
print(euler_angles)
|
||
|
||
|
||
|
||
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)
|
||
|
||
# 输出结果
|
||
ola=[]
|
||
for m in range(matrix_result.shape[0]):
|
||
ola.append(matrix_result[m][0])
|
||
print(f"{matrix_result[m][0]:<20}")
|
||
|
||
return ola
|
||
|
||
#这个版本是先拿法向量转换成基坐标，在转欧拉
|
||
#黄老师会给我传目标物的中心点坐标x,y,z和目标位姿的平面法向量a,b,c
|
||
def getPosition(x,y,z,a,b,c):
|
||
target = np.asarray([x, y, z])
|
||
camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
|
||
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)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
|
||
|
||
target_angle = vec2ola(dot_angle)#把转换之后的法向量转换为欧拉角,欧拉角不是rpy角
|
||
|
||
# r,p,y = angle_tool.as_euler('xyz',degrees=True)#r表示u,p表示v，y表示w
|
||
# target_angle = np.asarray([r,p,y])
|
||
# print(target_angle)
|
||
|
||
return target_position,target_angle |