队列运动接口

1. queue_motion_set_status - 打开、关闭队列运动模式

方法: queue_motion_set_status(socket_fd: int, status: bool)

功能描述: 切换控制器内部的高级队列缓冲运行模式。在批量压入高速、高频轨迹路点指令前,必须先将此状态配置为 True,以告知机械臂插补器切换内部运行机制,挂起实时运动并进入平滑路径队列等待状态。当轨迹执行完毕或需要退回普通单点运动时,应配置为 False 关闭该模式。

参数:

  • socket_fd:已连接的机械臂常规指令通道句柄,整型格式(int)。
  • status:队列运行状态使能标志,布尔格式(bool)。
    • True:激活/开启队列运行机制,后续发送的兼容运动指令将暂存至本地缓冲区。
    • False:关闭/退出队列运行机制,系统恢复为常规单步指令即时插补响应。

返回值:

  • int:执行结果
    • 0:执行成功。代表队列模式成功切换。
    • 非 0 整型值:执行失败。代表控制器当前状态不允许切换(例如机械臂当前处于物理运动中,硬性拦截模式切换)。
    • -1:通信异常。常规控制通道句柄无效或触发底层网络中断。

使用示例:

Python
import nrc_interface as nrc
import time

# 1. 设定测试参数:开启队列运动模式
test_status = True

# 2. 调用底层核心转换接口进行机制切换
ret = nrc.queue_motion_set_status(socket_fd, test_status)

# 3. 校验机制变更状态
if ret == 0:
    print("成功: 控制器已进入队列监听状态,后续可以压入密集路点数据流。")
else:
    print(f"错误: 切换队列运动模式失败,底层错误码: {ret}。请确保机械臂当前处于静止就绪态。")

time.sleep(2)

2. queue_motion_send_to_controller - 将本地队列的前size个数据发送到控制器

方法: queue_motion_send_to_controller(socket_fd: int, size: int, is_continue: bool)

功能描述: 将上位机本地缓冲池中暂存的密集运动指令数据流,一次性打包批量推送到控制器的高速执行缓冲区中,并立即命令机械臂启动物理运转。该机制通过批量异步吞吐,有效避免了单条连续下发时因网络抖动或指令解析延迟导致的机械臂物理运转顿挫。

参数:

  • socket_fd:已连接的机械臂常规指令通道句柄,整型格式(int)。
  • size:本次指令下发点数限额,整型格式(int)。有效取值范围为 0 ~ 31。
    • 0:全量推送。代表将当前本地缓冲池中的所有路点指令不设限地一次性全部下发至控制器。
    • 1 ~ 31:定量推送。代表仅下发当前缓冲区中前 size 个路点指令。
  • is_continue:断点继续执行控制,布尔格式(bool)。
    • True:若上一次队列因外部轻微报警或暂停挂起,保持当前物理姿态,从上次未完成的残余队列断点处继续向后执行。
    • False:清除历史残留状态,作为全新的独立运动轨迹从头刷新执行。

返回值:

  • int:状态码
    • 0:执行成功。代表队列数据成功被控制器协议栈接收并触发运动。
    • 非 0 整型值:执行失败。代表参数越界(如 size 溢出限制)或控制器高速缓冲区当前满载遭硬性拒绝。
    • -2 / -1:断连异常或未捕获的运行时异常。

使用示例:

Python
import nrc_interface as nrc
import time

# 1. 设定测试参数:全量发送本地队列,不从历史断点继续
test_size = 0
test_continue = False

# 2. 调用底层接口将暂存的轨迹路点打包推送到控制器
ret = nrc.queue_motion_send_to_controller(socket_fd, test_size, test_continue)

# 3. 校验队列注入状态
if ret == 0:
    print("成功: 密集路点数据已安全注入控制器硬件,机械臂已开始高平滑连续运行。")
else:
    print(f"错误: 队列数据推送失败,底层错误码: {ret}。")

time.sleep(2)

3. queue_motion_push_back_moveJ - 队列运动模式的本地队列最后插入一条moveJ运动

方法: queue_motion_push_back_moveJ(socket_fd: int, move_cmd: MoveCmd)

功能描述: 在本地内存缓冲区创建并向运动队列尾部追加(Push Back)一条 MoveJ(关节运动)路点命令。该指令下发后,数据会暂时驻留在本地,机械臂不会立刻产生物理动作。只有当后续调用 execute_queue 触发下发时,控制器才会将队列中的所有 MoveJ、MoveL 等路点连接成一条连续平滑的高速轨迹进行整体插补。

参数:

  • socket_fd:已连接的机械臂常规指令通道句柄,整型格式(int)。
  • move_cmd(底层 C++ 结构体):由 nrc.MoveCmd() 实例化的运动控制复合结构体,内部包含以下核心属性:
    • coord:坐标系代号(0: 关节坐标系, 1: 直角基座系)。
    • targetPosType:点位数据源类型,固定赋值为底层常量 nrc.PosType_data。
    • targetPosValue:开辟的 C++ 浮点容器 nrc.VectorDouble,顺序承载目标点位数据。
    • velocity / acc / dec:动态设定的该线段轨迹的限速及加减速百分比权重(1 ~ 100)。
    • pl:平滑等级(过渡半径代号)。在队列模式中默认设为 0,底层算法会自动完成路点间的连续速度平滑衔接。

返回值:

  • int:状态码
    • 0:压栈成功。代表该路点指令成功编入本地轨迹队列。
    • 非 0 整型值:压栈失败。代表内存结构体配置错误,或者本地分配的缓存队列长度已达上限。
    • -2 / -1:断连异常或未捕获的运行时异常。

使用示例:

Python
import nrc_interface as nrc
import time

# 1. 实例化 C++ 结构体和数据向量
target_joint = [0.0, -20.0, 90.0, 0.0, 45.0, 0.0]
pos_vector = nrc.VectorDouble(len(target_joint))
for i, v in enumerate(target_joint):
    pos_vector[i] = float(v)

cmd = nrc.MoveCmd()
cmd.coord = 0  # 关节系
cmd.targetPosType = nrc.PosType_data
cmd.targetPosValue = pos_vector
cmd.velocity = 15
cmd.acc = 30
cmd.dec = 30
cmd.pl = 0

# 2. 调用底层接口压入队列
ret = nrc.queue_motion_push_back_moveJ(socket_fd, cmd)

# 3. 校验压入状态
if ret == 0:
    print("成功: MoveJ 目标路点已编入本地队列,等待批量下发。")
else:
    print(f"错误: 无法将该点压入运动队列,底层错误码: {ret}")

# 完整队列运动模型见示例7

4. queue_motion_push_back_moveL - 队列运动模式的本地队列最后插入一条moveL运动

方法: queue_motion_push_back_moveL(socket_fd: int, move_cmd: MoveCmd)

功能描述: 在本地内存缓冲区创建并向运动队列尾部追加(Push Back)一条 MoveL(空间直线运动)路点命令。下发后,数据仍保持在本地,机械臂不会产生实际物理运动。MoveL 保证了机械臂末端 TCP(工具中心点)在空间中以严格的直线轨迹向目标过渡。

参数:

  • socket_fd:已连接的机械臂常规指令通道句柄,整型格式(int)。
  • move_cmd(底层 C++ 结构体):由 nrc.MoveCmd() 实例化的运动控制复合结构体,内部包含以下核心属性:
    • coord:坐标系代号(直线轨迹通常硬性指定为 1 基座直角坐标系,或 3 用户直角坐标系)。
    • targetPosType:点位数据源类型,固定赋值为底层常量 nrc.PosType_data。
    • targetPosValue:开辟的 C++ 浮点容器 nrc.VectorDouble,顺序承载 6 维空间目标位姿 [X, Y, Z, RX, RY, RZ](mm 和 rad)。
    • velocity / acc / dec:动态设定的该线段直线运动的速度及加减速百分比权重(1 ~ 100)。
    • pl:平滑等级(过渡半径代号)。在队列模式中默认设为 0,底层算法会自动完成前后多段线之间的连续平滑过渡。

返回值:

  • int:状态码
    • 0:压栈成功。代表该直线路点指令成功编入本地轨迹队列。
    • 非 0 整型值:压栈失败。代表输入参数或直角点位数据格式不满足机器人运动学逆解规范。
    • -2 / -1:断连异常或未捕获的运行时异常。

使用示例:

Python
import nrc_interface as nrc
import time

# 1. 实例化 C++ 结构体和空间直角数据向量
target_cartesian = [350.0, 0.0, 400.0, 3.1416, 0.0, 0.0]  # 直角空间位姿
pos_vector = nrc.VectorDouble(len(target_cartesian))
for i, v in enumerate(target_cartesian):
    pos_vector[i] = float(v)

cmd = nrc.MoveCmd()
cmd.coord = 1  # 显式采用基座直角坐标系
cmd.targetPosType = nrc.PosType_data
cmd.targetPosValue = pos_vector
cmd.velocity = 10
cmd.acc = 20
cmd.dec = 20
cmd.pl = 0

# 2. 调用底层接口压入 MoveL 队列
ret = nrc.queue_motion_push_back_moveL(socket_fd, cmd)

# 3. 校验压入状态
if ret == 0:
    print("成功: MoveL 直线路点已编入本地队列,等待执行。")
else:
    print(f"错误: 无法将该直线点压入运动队列,底层错误码: {ret}")

# 完整队列运动模型见示例7

5. queue_motion_push_back_moveC - 队列运动模式的本地队列最后插入一条moveC运动

方法: queue_motion_push_back_moveC(socket_fd: int, move_cmd: MoveCmd)

功能描述: 在本地内存缓冲区创建并向运动队列尾部追加(Push Back)一条 MoveC(三点圆弧运动)路点命令。下发后,机械臂处于静止监听状态,不会立刻物理运动。MoveC 算法通过机器人当前的物理位置(起点)、指令传入的中间点以及目标终点,在三维空间中拟合出唯一的圆弧轨迹。

参数:

  • socket_fd:已连接的机械臂常规指令通道句柄,整型格式(int)。
  • move_cmd(底层 C++ 结构体):由 nrc.MoveCmd() 实例化的运动控制复合结构体,内部包含以下核心属性:
    • coord:坐标系代号(圆弧运动通常硬性指定为 1 基座直角坐标系,或 3 用户直角坐标系)。
    • targetPosType:点位数据源类型,固定赋值为底层常量 nrc.PosType_data。
    • targetPosValue:开辟的 C++ 浮点容器 nrc.VectorDouble。必须填入 12 维数据:
      [0:6]位:圆弧中间过渡点 [Xm, Ym, Zm, RXm, RYm, RZm]
      [6:12]位:圆弧运动终点 [Xe, Ye, Ze, RXe, RYe, RZe]
    • velocity / acc / dec:动态设定的该圆弧线段的速度及加减速百分比权重(1 ~ 100)。
    • pl:平滑等级。在队列模式中默认设为 0,底层算法会自动完成路点间的连续速度平滑衔接。

返回值:

  • int:状态码
    • 0:压栈成功。代表该圆弧路点指令成功编入本地轨迹队列。
    • 非 0 整型值:压栈失败。代表输入数据不是 12 维,或者三点共线无法构成合法圆弧。
    • -2 / -1:断连异常或未捕获的运行时异常。

使用示例:

Python
import nrc_interface as nrc
import time

# 1. 准备 12 维复合点位:前 6 维为中间点,后 6 维为终点
mid_point = [300.0, 100.0, 400.0, 3.1416, 0.0, 0.0]
end_point = [250.0, 200.0, 400.0, 3.1416, 0.0, 0.0]
combined_pos = mid_point + end_point

pos_vector = nrc.VectorDouble(len(combined_pos))
for i, v in enumerate(combined_pos):
    pos_vector[i] = float(v)

# 2. 配置底层结构体
cmd = nrc.MoveCmd()
cmd.coord = 1  # 基座直角系
cmd.targetPosType = nrc.PosType_data
cmd.targetPosValue = pos_vector
cmd.velocity = 10
cmd.acc = 20
cmd.dec = 20
cmd.pl = 0

# 3. 调用底层接口压入 MoveC 队列
ret = nrc.queue_motion_push_back_moveC(socket_fd, cmd)

# 4. 校验压入状态
if ret == 0:
    print("成功: MoveC 三点圆弧路点已成功编入本地队列。")
else:
    print(f"错误: 无法将该圆弧点压入运动队列,底层错误码: {ret}")

# 注意:moveC 的前一条指令应该是 movej 或者 movel
# 完整队列运动模型见示例7

6. queue_motion_push_back_moveS - 队列运动模式的本地队列最后插入一条moveS运动

方法: queue_motion_push_back_moveS(socket_fd: int, move_cmd: MoveCmd)

功能描述: 在本地内存缓冲区创建并向运动队列尾部追加(Push Back)一条 MoveS(B样条/Cubic Spline 样条曲线)路点命令。下发后,数据仍驻留本地,机械臂不会产生实际物理运动。该接口是描述非规则、平滑曲面循迹的专用通道。当队列中连续压入多条 MoveS 指令并调用 execute_queue 下发时,控制器内部的数学插补器会自动将这些离散的点作为控制点,拟合出一条速度连续(C2 连续)、无拐点跳变的高阶平滑空间曲线。

参数:

  • socket_fd:已连接的机械臂常规指令通道句柄,整型格式(int)。
  • move_cmd(底层 C++ 结构体):由 nrc.MoveCmd() 实例化的运动控制复合结构体,内部包含以下核心属性:
    • coord:坐标系代号(样条曲线通常硬性指定为 1 基座直角坐标系,或 3 用户直角坐标系)。
    • targetPosType:点位数据源类型,固定赋值为底层常量 nrc.PosType_data。
    • targetPosValue:开辟的 C++ 浮点容器 nrc.VectorDouble,顺序承载 6 维空间目标位姿 [X, Y, Z, RX, RY, RZ](mm 和 rad)。
    • velocity / acc / dec:动态设定的该曲线段的速度及加减速百分比权重(1 ~ 100)。
    • pl:平滑等级。在样条曲线运动中固定设为 0,底层曲线拟合算法本身具有天然的多点高级平滑特性。

返回值:

  • int:执行结果
    • 0:压栈成功。代表该样条路点指令成功编入本地轨迹队列。
    • 非 0 整型值:压栈失败。代表输入参数或直角点位数据格式不满足机器人运动学逆解规范。
    • -2 / -1:断连异常或未捕获的运行时异常。

使用示例:

Python
import nrc_interface as nrc
import time

# 1. 拟合一条波浪线,准备 3 个连续控制点的直角坐标数据
spline_points = [
    [300.0, 50.0,  400.0, 3.1416, 0.0, 0.0], # 控制点 1
    [320.0, 100.0, 410.0, 3.1416, 0.0, 0.0], # 控制点 2
    [350.0, 150.0, 400.0, 3.1416, 0.0, 0.0]  # 控制点 3
]
success_count = 0

# 2. 通过循环,将所有控制点逐一压入本地缓冲区
for pt in spline_points:
    pos_vector = nrc.VectorDouble(len(pt))
    for i, v in enumerate(pt):
        pos_vector[i] = float(v)

    cmd = nrc.MoveCmd()
    cmd.coord = 1  # 基座直角系
    cmd.targetPosType = nrc.PosType_data
    cmd.targetPosValue = pos_vector
    cmd.velocity = 10
    cmd.acc = 20
    cmd.dec = 20
    cmd.pl = 0

    ret = nrc.queue_motion_push_back_moveS(socket_fd, cmd)
    if ret == 0:
        success_count += 1

print(f"成功: 已将 {success_count} 个 MoveS 控制点编入本地样条曲线队列,等待下发。")

# 注意:moveS 的前一条指令应该是 movej 或者 movel
# 完整队列运动模型见示例7