通讯与算法接口

1 set_receive_error_or_warnning_message_callback - 设置错误警告回调

方法： set_receive_error_or_warnning_message_callback(socket_fd:int, callback_func:function)

参数：

socket_fd：连接句柄
callback_func：回调函数

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

Python

import nrc_interface as nrc
#建立连接
socket_fd = nrc.connect_robot("192.168.1.13", "6001")
#先定义回调函数
def error_warning_callback(messageType, message, messageCode):
    print(f"[回调]类型={messageType},代码={messageCode},信息={message.decode('utf-8','ignore')}")
#直接调用接口
ret = nrc.set_receive_error_or_warnning_message_callback(socket_fd, error_warning_callback)
print(f"设置错误警告回调返回值(ret)={ret}")

2 modbus_set_master_parameter - 新增ModbusTCP主站

方法： modbus_set_master_parameter(socket_fd:int, slave_id:int, param:ModbusMasterParameter)

参数：

socket_fd：连接句柄
slave_id：从站ID
param：Modbus主站参数结构体

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

Python

import nrc_interface as nrc
#建立连接
socket_fd = nrc.connect_robot("192.168.1.13", "6001")
#创建参数对象
param = nrc.ModbusMasterParameter()
#调用接口设置/获取
ret = nrc.modbus_set_master_parameter(socket_fd, 1, param)
print(f"设置Modbus主站参数返回值(ret)={ret}")
#打印所有参数
print("====================ModbusMasterParameter====================")
print(f"type={param.type}")
print(f"startAddress={param.startAddress}")
print(f"TCP={param.TCP}")
print(f"RTU={param.RTU}")

3 get_origin_coord_to_target_coord - 正解算法

方法： get_origin_coord_to_target_coord_robot(socket_fd:int, robot_id:int, origin_coord:int, origin_pos:VectorDouble, target_coord:int, target_pos:VectorDouble)

参数：

socket_fd：连接句柄
robot_id：机器人ID
origin_coord：原坐标系
- 0：关节坐标系
- 1：直角坐标系
- 2：工具坐标系
- 3：用户坐标系
origin_pos：原坐标值
target_coord：目标坐标系
target_pos：输出参数，用于存储转换后的坐标值

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

Python

import nrc_interface as nrc
#建立连接
socket_fd = nrc.connect_robot("192.168.1.13", "6001")
#坐标转换：原坐标系→目标坐标系
#原坐标系：0关节 1直角 2工具 3用户
originCoord = 1
#要转换的原始坐标（7位：x,y,z,rx,ry,rz,0）
originPos = nrc.VectorDouble()
originPos.push_back(0.0)   # x
originPos.push_back(0.0)   # y
originPos.push_back(780.0) # z
originPos.push_back(0.0)   # rx
originPos.push_back(0.0)   # ry
originPos.push_back(-3.14) # rz
originPos.push_back(0.0)   # 第7位
#目标坐标系
targetCoord = 0
#存储转换结果
targetPos = nrc.VectorDouble()
for _ in range(7):
    targetPos.push_back(0.0)
#直接调用函数
ret = nrc.get_origin_coord_to_target_coord_robot(
    socket_fd,
    1,
    originCoord,
    originPos,
    targetCoord,
    targetPos
)
#打印
print(f"坐标转换返回值={ret}")
print(f"原坐标={list(originPos)}")
print(f"转换后坐标={list(targetPos)}")

4 get_quat2rpy - 四元数转欧拉角

方法： get_quat2rpy(socketFd, quat_vector, rpy_res)

参数：

socketFd：连接句柄
quat_vector：被转换的四元数
rpy_res：接收欧拉角结果

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

统一参考示例9

5 get_rpy2quat - 欧拉角转四元数

方法： get_quat2rpy(socketFd, quat_vector, rpy_res)

参数：

socketFd：连接句柄
quat_vector：被转换的欧拉角
rpy_res：接收四元素的结果

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

参考示例9

6 get_rpy2r - 欧拉角转旋转矩阵

方法： get_rpy2r(socketFd, rpy_vector, r_res)

参数：

socketFd：连接句柄
rpy_vector：被转换的欧拉角
r_res：接收旋转矩阵的结果

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

参考示例9

7 get_tr2r - 位姿转旋转矩阵

方法： get_tr2r(socketFd, tr_matrix, r_res)

参数：

socketFd：连接句柄
tr_matrix：被转换的位姿
res：接收旋转矩阵的结果

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

参考示例9

8 get_r2tr - 旋转矩阵转位姿矩阵

方法： get_r2tr(socketFd, r_matrix, tr_res)

参数：

socketFd：连接句柄
r_matrix：被转换的旋转矩阵
tr_res：接收位姿矩阵的结果

返回值：

int：执行结果
- 0：成功
- 非0：失败，错误码见错误码详情

使用示例：

参考示例9

9 欧拉角，四元素，位姿矩阵，旋转矩阵相互转化使用示例

Python

import nrc_interface as nrc
import time

class ArmTest:
    #  ======================    四元数  转  欧拉角  ======================
    def get_quat2rpy(self, qx, qy, qz, qw, socket_fd=None):
        fd = socket_fd
        quat_vec = nrc.VectorDouble(4)
        quat_vec[0] = qx
        quat_vec[1] = qy
        quat_vec[2] = qz
        quat_vec[3] = qw

        rpy_vec = nrc.VectorDouble(3)
        ret = nrc.get_quat2rpy(fd, quat_vec, rpy_vec)

        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_vec = nrc.VectorDouble(3)
        rpy_vec[0] = rx
        rpy_vec[1] = ry
        rpy_vec[2] = rz

        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_vec = nrc.VectorDouble(3)
        rpy_vec[0] = rx
        rpy_vec[1] = ry
        rpy_vec[2] = rz

        r_vec = nrc.VectorDouble(9)
        ret = nrc.get_rpy2r(fd, rpy_vec, r_vec)

        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}")
        return ret, r00, r01, r02, r10, r11, r12, r20, r21, r22

    #  位姿  转  旋转矩阵
    def get_rt2r(self, tr_matrix, socket_fd=None):
        fd = socket_fd
        tr_vec = nrc.VectorDouble(16)
        for i in range(16):
            tr_vec[i] = tr_matrix[i]

        r_vec = nrc.VectorDouble(9)
        ret = nrc.get_tr2r(fd, tr_vec, r_vec)

        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}")
        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
        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

        tr_res = nrc.VectorDouble(16)
        ret = nrc.get_r2tr(fd, r_vec, tr_res)

        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}")
        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)