更新 Trace/handeye_calibration.py
This commit is contained in:
cdeyw
@ -1,97 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
rotation = R_matrix()#张啸给我的值填这里
|
|
||||||
|
|
||||||
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):#首先是相机到法兰的转换，之后是法兰到新坐标系的转换（新坐标系就是与Z轴与法向量一致的坐标系)
|
|
||||||
#如果不转换坐标轴，
|
|
||||||
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)#或者是zxy
|
|
||||||
|
|
||||||
# print("Euler Angles (degrees):")
|
|
||||||
# print(euler_angles)
|
|
||||||
return euler_angles
|
|
||||||
|
|
||||||
#张啸给我的xyzuvw
|
|
||||||
def R_matrix(x,y,z,u,v,w):
|
|
||||||
rx = np.radians(u)
|
|
||||||
ry = np.radians(v)
|
|
||||||
rz = np.radians(w)
|
|
||||||
# 定义绕 X, Y, Z 轴的旋转矩阵
|
|
||||||
R_x = np.array([
|
|
||||||
[1, 0, 0],
|
|
||||||
[0, np.cos(rx), -np.sin(rx)],
|
|
||||||
[0, np.sin(rx), np.cos(rx)]
|
|
||||||
])
|
|
||||||
|
|
||||||
R_y = np.array([
|
|
||||||
[np.cos(ry), 0, np.sin(ry)],
|
|
||||||
[0, 1, 0],
|
|
||||||
[-np.sin(ry), 0, np.cos(ry)]
|
|
||||||
])
|
|
||||||
|
|
||||||
R_z = np.array([
|
|
||||||
[np.cos(rz), -np.sin(rz), 0],
|
|
||||||
[np.sin(rz), np.cos(rz), 0],
|
|
||||||
[0, 0, 1]
|
|
||||||
])
|
|
||||||
R = R_z @ R_y @ R_x
|
|
||||||
T = np.array([x, y, z])
|
|
||||||
|
|
||||||
# 构建齐次变换矩阵
|
|
||||||
transformation_matrix = np.eye(4)
|
|
||||||
transformation_matrix[:3, :3] = R
|
|
||||||
transformation_matrix[:3, 3] = T
|
|
||||||
|
|
||||||
return transformation_matrix
|
|
||||||
|
|
||||||
|
|
||||||
# 黄老师给我的xyz和法向量
|
|
||||||
def getPosition(x,y,z,a,b,c):
|
|
||||||
target = np.asarray([x, y, z,1])
|
|
||||||
camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
|
|
||||||
robot2base = rotation
|
|
||||||
camera2base = robot2base @ camera2robot
|
|
||||||
target_position = np.dot(camera2base, target)
|
|
||||||
|
|
||||||
rpy = vec2ola(a, b, c)
|
|
||||||
r, p, y = rpy[0], rpy[1], rpy[2] # r表示u,p表示v，y表示w
|
|
||||||
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
|
|
||||||
80
Trace/handeye_calibration.py
Normal file
80
Trace/handeye_calibration.py
Normal file
@ -0,0 +1,80 @@
|
|||||||
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation as R
|
||||||
|
|
||||||
|
def vec2rpy(normal):#首先是相机到法兰的转换，之后是法兰到新坐标系的转换（新坐标系就是与Z轴与法向量一致的坐标系)
|
||||||
|
# 将法向量作为机械臂末端执行器的新X轴
|
||||||
|
x_axis = normal / np.linalg.norm(normal) # 归一化
|
||||||
|
|
||||||
|
# 选择一个合适的Y轴方向，尽量避免与X轴共线
|
||||||
|
if abs(x_axis[2]) != 1:
|
||||||
|
y_axis = np.array([0, 1, 0])
|
||||||
|
else:
|
||||||
|
y_axis = np.array([0, 0, 1])
|
||||||
|
|
||||||
|
y_axis = y_axis - np.dot(y_axis, x_axis) * x_axis # 投影到垂直于x轴的平面
|
||||||
|
y_axis = y_axis / np.linalg.norm(y_axis)
|
||||||
|
|
||||||
|
# 计算Z轴方向，确保它与X轴和Y轴正交
|
||||||
|
z_axis = np.cross(x_axis, y_axis)
|
||||||
|
|
||||||
|
# 构造旋转矩阵
|
||||||
|
rotation_matrix = np.vstack([x_axis, y_axis, z_axis]).T
|
||||||
|
|
||||||
|
# 将旋转矩阵转换为RPY（roll, pitch, yaw）
|
||||||
|
rpy = R.from_matrix(rotation_matrix).as_euler('xyz', degrees=True)
|
||||||
|
|
||||||
|
return rpy
|
||||||
|
|
||||||
|
#张啸给我的xyzuvw
|
||||||
|
def R_matrix(x,y,z,u,v,w):
|
||||||
|
rx = np.radians(u)
|
||||||
|
ry = np.radians(v)
|
||||||
|
rz = np.radians(w)
|
||||||
|
# 定义绕 X, Y, Z 轴的旋转矩阵
|
||||||
|
R_x = np.array([
|
||||||
|
[1, 0, 0],
|
||||||
|
[0, np.cos(rx), -np.sin(rx)],
|
||||||
|
[0, np.sin(rx), np.cos(rx)]
|
||||||
|
])
|
||||||
|
|
||||||
|
R_y = np.array([
|
||||||
|
[np.cos(ry), 0, np.sin(ry)],
|
||||||
|
[0, 1, 0],
|
||||||
|
[-np.sin(ry), 0, np.cos(ry)]
|
||||||
|
])
|
||||||
|
|
||||||
|
R_z = np.array([
|
||||||
|
[np.cos(rz), -np.sin(rz), 0],
|
||||||
|
[np.sin(rz), np.cos(rz), 0],
|
||||||
|
[0, 0, 1]
|
||||||
|
])
|
||||||
|
R = R_z @ R_y @ R_x
|
||||||
|
T = np.array([x, y, z])
|
||||||
|
|
||||||
|
# 构建齐次变换矩阵
|
||||||
|
transformation_matrix = np.eye(4)
|
||||||
|
transformation_matrix[:3, :3] = R
|
||||||
|
transformation_matrix[:3, 3] = T
|
||||||
|
|
||||||
|
return transformation_matrix
|
||||||
|
|
||||||
|
rotation = R_matrix(559.365,0.704,731.196,179.996,66.369,179.996)#张啸给我的值填这里
|
||||||
|
|
||||||
|
# 黄老师给我的xyz和法向量
|
||||||
|
def getPosition(x,y,z,a,b,c):
|
||||||
|
target = np.asarray([x, y, z,1])
|
||||||
|
camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
|
||||||
|
robot2base = rotation
|
||||||
|
camera2base = robot2base @ camera2robot
|
||||||
|
target_position = np.dot(camera2base, target)
|
||||||
|
|
||||||
|
angle = np.asarray([a,b,c])
|
||||||
|
noraml = camera2base[:3, :3]@angle
|
||||||
|
noraml_base = vec2rpy(noraml)
|
||||||
|
|
||||||
|
print(target_position, noraml_base)
|
||||||
|
|
||||||
|
return target_position,noraml_base
|
||||||
|
|
||||||
|
if __name__ =="__main__":
|
||||||
|
getPosition(-96.09919,56.339145,444.640084,-0.09619,-0.45399, 0.88579)
|
||||||
Reference in New Issue
Block a user