7000端口使用方法

1. open_servoJ - 打开关节跟踪模式

方法： open_servoJ(socket_fd: int, vmax_vec: VectorDouble, amax_vec: VectorDouble, jmax_vec: VectorDouble)

功能描述： 通知机器人控制器切换插补引擎为高频流式跟踪模式（ServoJ 模式）。在此模式打开后，控制器会挂起常规的轨迹规划器，并向 socket_fd 通道解锁高速底层透传接口。外部上位机可以通过固定周期（如 4ms - 20ms）持续下发点位数据，实现外部算法对机械臂进行微秒级的动态轨迹修正。

参数：

socket_fd：已连接的机械臂常规指令通道句柄，整型格式（int）。
vmax_vec（底层输入参数）：C++ 双精度浮点向量容器 nrc.VectorDouble，内部顺序存储了 1~6 轴每个关节允许的最大运行速度约束（°/s）。
amax_vec（底层输入参数）：C++ 双精度浮点向量容器 nrc.VectorDouble，内部顺序存储了 1~6 轴每个关节允许的最大运行加速度约束（°/s²）。
jmax_vec（底层输入参数）：C++ 双精度浮点向量容器 nrc.VectorDouble，内部顺序存储了 1~6 轴每个关节允许的最大加加速度（Jerk，冲击力）约束（°/s³）。

返回值：

int：执行结果
- 0：激活成功。代表控制器成功进入高速流式透传就绪态。
- 非 0 整型值：激活失败。代表系统当前有未清除的报警，或者当前运行模式不允许进入高频控制。
- -2 / -1：断连异常或未捕获的运行时异常。

使用示例：

Python

import nrc_interface as nrc

# 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 控制点编入本地样条曲线队列，等待下发。")

2. close_servoJ - 关闭关节跟踪模式

方法： close_servoJ(socket_fd: int)

功能描述： 通知机器人控制器注销当前的高频流式跟踪模式（ServoJ 模式）。执行该接口后，控制器会安全关闭专属于 7000 端口底层的高速透传数据监听回路，重新激活内部的全局轨迹规划器和常规状态机。

参数：

socket_fd：已连接的机械臂高速实时跟踪控制通道（通常固定为 7000 端口）句柄，整型格式（int）。

返回值：

int：状态码
- 0：注销成功。代表控制器已安全退出高速流式透传模式，重返待机状态。
- 非 0 整型值：注销失败。代表当前控制器核心忙，或者状态机发生不可逆的底层死锁错误。
- -2 / -1：通道断连异常或未捕获的运行时异常。

使用示例：

Python

import nrc_interface as nrc

# 1. 外部高频循环运动（如视觉追踪任务）正常结束，跳出循环
print("通知: 实时流控制任务已结束，准备关闭 ServoJ 监听通道...")

# 2. 调用底层退场原语，传入 7000 端口状态句柄
ret = nrc.close_servoJ(robotarm7000)

# 3. 校验机制退场状态
if ret == 0:
    print("成功: 伺服跟踪回路已安全释放，机械臂电机仍保持使能，可继续下发常规 Move 指令。")
else:
    print(f"错误: 注销伺服跟踪模式失败，底层错误码: {ret}。")

3. set_servoJ_pos - 设置实时跟踪关节位置

方法： set_servoJ_pos(socket_fd: int, q_vec: VectorDouble)

功能描述： 在通过 open_servo_joint 激活了伺服跟踪机制后，该接口作为高频动态点位的实时发射通道。上位机通常在独立的定时器线程中，以固定的硬件时间周期（如 4ms / 8ms / 12ms）不间断地向 7000 端口疯狂推送当前的增量位置。控制器接收到点位后，不会进行路径减速平滑，而是以极高地动态响应率直接驱动物理马达追赶该目标位置。

参数：

socket_fd：已连接的机械臂高速实时跟踪控制通道（通常固定为 7000 端口）句柄，整型格式（int）。
q_vec（底层输入参数）：专门用于承载多轴目标角度的双精度浮点向量容器（对应 nrc.VectorDouble()）。依据控制器运动学引擎设计规范，其内存长度必须满足 7 维要求。对于 6 自由度机器人，第 7 位（q_vec[6]）必须通过 push_back(0.0) 显式填充占位符，否则底层 C++ 数组指针在解算雅可比矩阵时会因寻址越界引发内存崩溃。

返回值：

int：状态码
- 0：数据成功发送至控制器缓冲区并被插补器接管。
- 非 0 整型值：发送失败。代表当前点位突变过大（超出了安全速度红线限制）、点位超出了物理软限位、或者系统未处于 ServoJ 开启状态。
- -2 / -1：特定通道网络断开或运行时异常。

使用示例：

Python

import time
import nrc_interface as nrc

# 假设此段代码处于一个每 8ms 触发一次的高精度定时器循环回调函数中
# current_angles 是从外部视觉传感器或 AI 轨迹算法动态实时计算出的 6 轴新目标 [q1, q2, q3, q4, q5, q6]
current_angles = [0.0, -20.0, 90.0, 0.0, 45.12, 0.0]

# 1. 创建底层高频容器并填充
q_vec = nrc.VectorDouble()
for val in current_angles:
    q_vec.push_back(float(val))
q_vec.push_back(0.0)  # 硬性补齐第 7 维，保护底层内存

# 2. 通过 7000 端口专用句柄下发
ret = nrc.set_servoJ_pos(robotarm7000, q_vec)

# 3. 仅对错误进行条件拦截捕获，正常情况下放行以保证 I/O 吞吐率
if ret != 0:
    print(f"【高频警告】流式点位注入失败！底层报警码: {ret}")

# 完整关节实时跟踪 servoJ 使用示例测试代码见示例2。