用python写一段代码 已知工具的位姿x y z Rx Ry Rz 求机器人坐标系到工具坐标系的矩阵

好的，以下是一段 Python 代码，用于计算机器人坐标系到工具坐标系的变换矩阵：

import numpy as np

# 定义工具位姿
x = 100
y = 200
z = 300
Rx = 0.1
Ry = 0.2
Rz = 0.3

# 计算旋转矩阵
Rx_matrix = np.array([[1, 0, 0],
                      [0, np.cos(Rx), -np.sin(Rx)],
                      [0, np.sin(Rx), np.cos(Rx)]])
Ry_matrix = np.array([[np.cos(Ry), 0, np.sin(Ry)],
                      [0, 1, 0],
                      [-np.sin(Ry), 0, np.cos(Ry)]])
Rz_matrix = np.array([[np.cos(Rz), -np.sin(Rz), 0],
                      [np.sin(Rz), np.cos(Rz), 0],
                      [0, 0, 1]])
rotation_matrix = Rz_matrix @ Ry_matrix @ Rx_matrix

# 计算变换矩阵
transformation_matrix = np.eye(4)
transformation_matrix[:3, :3] = rotation_matrix
transformation_matrix[:3, 3] = np.array([x, y, z])

print(transformation_matrix)

在这个例子中，我们假设机器人坐标系与世界坐标系重合，并定义了工具在机器人坐标系下的位姿。首先，我们通过Rx、Ry和Rz计算出旋转矩阵，然后将其与位移向量[x, y, z]组合成变换矩阵。最后，我们打印出变换矩阵，该矩阵描述了从机器人坐标系到工具坐标系的转换。