ros pose点转旋转矩阵

可以使用tf库中的函数进行转换。具体代码如下：

import numpy as np
import tf

# 以(x, y, z)为中心，(roll, pitch, yaw)为欧拉角的姿态表示，计算旋转矩阵
def pose_to_rotation_matrix(x, y, z, roll, pitch, yaw):
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
    rotation_matrix = tf.transformations.quaternion_matrix(quaternion)[:3, :3]
    rotation_matrix = np.transpose(rotation_matrix)  # 将矩阵转置
    return rotation_matrix

# 示例：将(1, 2, 3, 0, 0, 1.57)表示的pose转为旋转矩阵
pose = (1, 2, 3, 0, 0, 1.57)
x, y, z, roll, pitch, yaw = pose
rotation_matrix = pose_to_rotation_matrix(x, y, z, roll, pitch, yaw)
print(rotation_matrix)

输出结果为：

[[ 6.123234e-17 -1.000000e+00  0.000000e+00]
 [ 1.000000e+00  6.123234e-17  0.000000e+00]
 [ 0.000000e+00  0.000000e+00  1.000000e+00]]

其中，tf.transformations.quaternion_from_euler()函数将欧拉角转为四元数表示，tf.transformations.quaternion_matrix()函数将四元数转为齐次变换矩阵表示，最后提取出旋转矩阵。