90 lines
2.8 KiB
Python
90 lines
2.8 KiB
Python
import numpy as np
|
||
from scipy.spatial.transform import Rotation as R
|
||
|
||
def vec2rpy(normal,short_edge_direction):
|
||
# 将法向量的反方向作为机械臂末端执行器的新Z轴
|
||
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)
|
||
|
||
y_axis = np.cross(z_axis,x_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
|
||
|
||
|
||
# 图像识别结果：xyz和法向量
|
||
def getPosition(x,y,z,a,b,c,rotation,points):
|
||
target = np.asarray([x, y, z,1])
|
||
camera2robot = np.loadtxt('./Trace/com_pose2.txt', delimiter=' ') #相对目录且分隔符采用os.sep
|
||
# robot2base = rotation
|
||
# camera2base = robot2base @ camera2robot
|
||
target_position_raw = np.dot(camera2robot, target)
|
||
|
||
corner_points_camera = np.asarray(points)
|
||
corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3]
|
||
# 按照 x 轴排序
|
||
sorted_points = corner_points_base[np.argsort(corner_points_base[:, 0])]
|
||
# 选出x轴较大的两个点
|
||
point_1 = sorted_points[-1] # x值较大的点
|
||
point_2 = sorted_points[-2] # x值较小的点
|
||
# 根据 y 值选择差值方向
|
||
if point_1[1] < point_2[1]:
|
||
edge_vector = point_1 - point_2
|
||
else:
|
||
edge_vector = point_2 - point_1
|
||
# 单位化方向向量
|
||
short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
|
||
|
||
delta = -30#沿法向量方向抬高和压低，-指表示抬高，+值表示压低
|
||
angle = np.asarray([a,b,c])
|
||
noraml = camera2robot[:3, :3]@angle
|
||
normal_vector = noraml / np.linalg.norm(noraml)
|
||
target_position = target_position_raw[:3] + delta * normal_vector # target_position 沿法向量移动
|
||
|
||
noraml_base = vec2rpy(noraml,short_edge_direction)
|
||
|
||
return target_position,noraml_base
|
||
|
||
|