Python
import nrc_interface as nrc
import time
class ArmTest:
# ====================== 四元数 转 欧拉角 ======================
def get_quat2rpy(self, qx, qy, qz, qw, socket_fd=None):
# 1. 创建输入四元数 vector（长度4）
fd = socket_fd
quat_vec = nrc.VectorDouble(4)
quat_vec[0] = qx
quat_vec[1] = qy
quat_vec[2] = qz
quat_vec[3] = qw
# 2. 创建输出欧拉角 vector（长度3）
rpy_vec = nrc.VectorDouble(3)
# 3. 调用接口
ret = nrc.get_quat2rpy(fd, quat_vec, rpy_vec)
# 4. 读取结果
rx = rpy_vec[0]
ry = rpy_vec[1]
rz = rpy_vec[2]
print(f"转换返回值 ret = {ret}")
print(f"欧拉角 Rx={rx:.3f}, Ry={ry:.3f}, Rz={rz:.3f}")
return ret, rx, ry, rz
# ====================== 欧拉角 转 四元数 ======================
def get_rpy2quat(self, rx, ry, rz, socket_fd=None):
fd = socket_fd
# 输入：欧拉角 RPY 长度 3
rpy_vec = nrc.VectorDouble(3)
rpy_vec[0] = rx
rpy_vec[1] = ry
rpy_vec[2] = rz
# 输出：四元数 长度 4
quat_vec = nrc.VectorDouble(4)
# 调用接口
ret = nrc.get_rpy2quat(fd, rpy_vec, quat_vec)
# 取出四元数结果
qx = quat_vec[0]
qy = quat_vec[1]
qz = quat_vec[2]
qw = quat_vec[3]
print(f"rpy2quat 返回值: {ret}")
print(f"四元数结果: qx={qx:.3f}, qy={qy:.3f}, qz={qz:.3f}, qw={qw:.3f}")
return ret, qx, qy, qz, qw
# 欧拉角 转 旋转矩阵
def get_rpy2r(self, rx, ry, rz, socket_fd=None):
fd = socket_fd
# 输入：欧拉角 RPY 长度 3
rpy_vec = nrc.VectorDouble(3)
rpy_vec[0] = rx
rpy_vec[1] = ry
rpy_vec[2] = rz
# 输出：旋转矩阵 长度 9（行主序）
r_vec = nrc.VectorDouble(9)
# 调用官方接口
ret = nrc.get_rpy2r(fd, rpy_vec, r_vec)
# 取出旋转矩阵 9 个值
r00 = r_vec[0]
r01 = r_vec[1]
r02 = r_vec[2]
r10 = r_vec[3]
r11 = r_vec[4]
r12 = r_vec[5]
r20 = r_vec[6]
r21 = r_vec[7]
r22 = r_vec[8]
# 打印
print(f"rpy2r 返回值: {ret}")
print(f"旋转矩阵结果: r00={r00:.3f}, r01={r01:.3f}, r02={r02:.3f}")
print(f" r10={r10:.3f}, r11={r11:.3f}, r12={r12:.3f}")
print(f" r20={r20:.3f}, r21={r21:.3f}, r22={r22:.3f}")
# 返回：ret + 9个矩阵值
return ret, r00, r01, r02, r10, r11, r12, r20, r21, r22
#位姿 转 旋转矩阵
def get_rt2r(self, tr_matrix, socket_fd=None):
fd = socket_fd
# 输入：位姿矩阵 长度 16（行主序）
tr_vec = nrc.VectorDouble(16)
for i in range(16):
tr_vec[i] = tr_matrix[i]
# 输出：旋转矩阵 长度 9（行主序）
r_vec = nrc.VectorDouble(9)
# 调用接口
ret = nrc.get_tr2r(fd, tr_vec, r_vec)
# 取出旋转矩阵 9 个值
r00 = r_vec[0]
r01 = r_vec[1]
r02 = r_vec[2]
r10 = r_vec[3]
r11 = r_vec[4]
r12 = r_vec[5]
r20 = r_vec[6]
r21 = r_vec[7]
r22 = r_vec[8]
# 打印
print(f"rt2r 返回值: {ret}")
print(f"旋转矩阵结果: r00={r00:.3f}, r01={r01:.3f}, r02={r02:.3f}")
print(f" r10={r10:.3f}, r11={r11:.3f}, r12={r12:.3f}")
print(f" r20={r20:.3f}, r21={r21:.3f}, r22={r22:.3f}")
# 返回格式ret + 9个矩阵值
return ret, r00, r01, r02, r10, r11, r12, r20, r21, r22
# 旋转矩阵 转 位姿矩阵
def get_r2tr(self, r00, r01, r02, r10, r11, r12, r20, r21, r22, socket_fd=None):
fd = socket_fd
# 输入：旋转矩阵 长度9（行主序）
r_vec = nrc.VectorDouble(9)
r_vec[0] = r00
r_vec[1] = r01
r_vec[2] = r02
r_vec[3] = r10
r_vec[4] = r11
r_vec[5] = r12
r_vec[6] = r20
r_vec[7] = r21
r_vec[8] = r22
# 输出：位姿矩阵 长度16（行主序）
tr_res = nrc.VectorDouble(16)
# 调用接口
ret = nrc.get_r2tr(fd, r_vec, tr_res)
# 取出位姿矩阵16个值
tr00 = tr_res[0]
tr01 = tr_res[1]
tr02 = tr_res[2]
tr03 = tr_res[3]
tr10 = tr_res[4]
tr11 = tr_res[5]
tr12 = tr_res[6]
tr13 = tr_res[7]
tr20 = tr_res[8]
tr21 = tr_res[9]
tr22 = tr_res[10]
tr23 = tr_res[11]
tr30 = tr_res[12]
tr31 = tr_res[13]
tr32 = tr_res[14]
tr33 = tr_res[15]
# 打印
print(f"r2tr 返回值: {ret}")
print(f"位姿矩阵结果: tr00={tr00:.3f}, tr01={tr01:.3f}, tr02={tr02:.3f}, tr03={tr03:.3f}")
print(f" tr10={tr10:.3f}, tr11={tr11:.3f}, tr12={tr12:.3f}, tr13={tr13:.3f}")
print(f" tr20={tr20:.3f}, tr21={tr21:.3f}, tr22={tr22:.3f}, tr23={tr23:.3f}")
print(f" tr30={tr30:.3f}, tr31={tr31:.3f}, tr32={tr32:.3f}, tr33={tr33:.3f}")
# 返回：ret + 16个位姿值
return ret, tr00, tr01, tr02, tr03, tr10, tr11, tr12, tr13, tr20, tr21, tr22, tr23, tr30, tr31, tr32, tr33
if __name__ == '__main__':
# 建立连接
arm_test = ArmTest()
socket_fd = nrc.connect_robot("192.168.1.13", "6001")
arm_test.get_rpy2quat(-2.9,0.167,-0.777,socket_fd)
arm_test.get_quat2rpy(-0.080, 0.919, 0.365, 0.122,socket_fd)
arm_test.get_rpy2r(-2.9,0.167,-0.777,socket_fd)
arm_test.get_r2tr(0.703, 0.691, 0.166, 0.652, -0.720, 0.236, 0.283, -0.057, -0.957, socket_fd)
tr = [
0.703, 0.691, 0.166, 0.000,
0.652, -0.720, 0.236, 0.000,
0.283, -0.057, -0.957, 0.000,
0.000, 0.000, 0.000, 1.000
]
arm_test.get_rt2r(tr, socket_fd )
