22 lines
980 B
Plaintext
22 lines
980 B
Plaintext
import numpy as np
|
||
from scipy.spatial.transform import Rotation as R
|
||
|
||
|
||
#黄老师会给我传目标物的中心点坐标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)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
|
||
|
||
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
|
||
target_angle = np.asarray([r,p,y])
|
||
|
||
return target_position, dot_angle, target_angle
|