纳博特科技开放平台

中文

English
한국어

中文

English
한국어

Appearance

🤖 AI 助手 / 自动化工具：先读 《AI 助手先读》 《Agent 开发指引》 《生成前信息采集清单》；机器可读入口 /llms.txt；技能包（SKILL）直链：SKILL.md

使用 servoJ 进行关节控制 ​

servoJ 是关节空间伺服跟随接口：由上位机按固定周期（官方示例 10 ms）逐个下发 7 维目标关节角，控制器按速度 / 加速度 / 加加速度约束平滑跟随。适合视觉、力控、传感器引导、遥操作、在线纠偏这类"外部实时给点"的场景；点位任务请使用 robot_movj / robot_movel 一类运动指令。

⚠️ 安全警告

以下代码会导致机器人实际运动。执行前必须确认：

  • 人员安全： 机器人工作空间内无人员或障碍物
  • 急停可用： 急停按钮在可触及范围内
  • 伺服使能： 确认伺服已正确使能
  • 速度限制： 首次运行将速度设置为较低值（建议 ≤ 最大速度的 20%）
  • 坐标系与单位： 关节运动单位为度（°），直线运动单位为毫米（mm）

建议先在仿真环境或空载低速条件下测试，确认无误后再用于实际生产。

适用场景与接口选型 ​

需求推荐接口说明
关节空间逐周期跟随（视觉 / 力控 / 遥操作 / 纠偏）open_servoJ / set_servoJ_pos / close_servoJ本页内容，调用方负责每周期给点
已有一串带时间戳的轨迹点servo_move一次给一组点，控制器按 timeStamp 平滑
伺服点位运动控制servo_point_position_motion_control逐周期点位下发
普通点位 / 直线运动robot_movj / robot_movel 等不需要逐周期喂点，见"基本应用"示例

运行前提 ​

两个端口 ​

端口用途本页是否必需
7000伺服跟踪数据通道（servoJ 专用）必需
6001命令 / 状态通道（清错、上下电、读坐标）必需

open_servoJ、set_servoJ_pos、close_servoJ 三个接口只接受 7000 端口的连接句柄，传入 6001 的句柄会返回 DISCONNECT（-2）。

cpp
SOCKETFD fd_6001 = connect_robot("192.168.1.13", "6001");   // 命令 / 状态
SOCKETFD fd_7000 = connect_robot("192.168.1.13", "7000");   // servoJ 专用

使能顺序 ​

cpp
clear_error(fd_6001);            // 有报警先清错
set_servo_state(fd_6001, 1);     // 伺服置 1 = 就绪（0=停止 1=就绪 2=报警 3=运行）
set_servo_poweron(fd_6001);      // 上电；成功后 get_servo_state 应为 3
  • set_servo_state 只在伺服状态为 0（停止）或 1（就绪）时生效，状态为 2（报警）或 3（运行时）不能直接设置。
  • set_servo_poweron 要求先调用 set_servo_state(fd, 1)。
  • 上下电类命令的返回值只表示命令已下发，不能据此认为已上电；建议上电后轮询 get_servo_state 直到等于 3 再开启 servoJ。

接口说明 ​

cpp
/**
 * @brief 打开关节跟踪模式
 * @param vmax 速度约束 单位 度/秒
 * @param amax 加速度约束 单位 度/秒^2
 * @param jmax 加加速度约束 单位 度/秒^3
 */
Result open_servoJ(SOCKETFD socketFd, std::vector<double> vmax, std::vector<double> amax, std::vector<double> jmax);

/**
 * @brief 发送跟踪关节位置
 * @param q 目标位置 单位 度
 */
Result set_servoJ_pos(SOCKETFD socketFd, std::vector<double> q);

/**
 * @brief 关闭关节跟踪模式
 */
Result close_servoJ(SOCKETFD socketFd);

参数说明 ​

接口参数类型说明
open_servoJsocketFdSOCKETFD必须是 7000 端口句柄
vmax / amax / jmaxstd::vector<double>速度（度/秒）/ 加速度（度/秒²）/ 加加速度（度/秒³）约束，长度必须 ≥ 7
set_servoJ_posqstd::vector<double>目标关节角（度），长度必须 ≥ 7；是目标绝对位置，不是增量
close_servoJsocketFdSOCKETFD7000 端口句柄

返回值 ​

值枚举含意
0SUCCESS成功（对 servoJ 而言表示命令已写入 socket）
-1RECEIVE_FAILED接收失败
-2DISCONNECT句柄无效，或该句柄不是 7000 端口（servoJ 最典型的失败）
-3PARAM_ERR参数错误
-4OPERATION_NOT_ALLOWED当前状态不允许
-5EXCEPTION异常
-6TIMEOUT超时

要点： servoJ 三个接口发送命令后立即返回，不等待控制器应答，因此返回 0 不代表控制器已接受、更不代表机器人已经在动。要确认效果，请用 6001 端口的 get_current_position / get_servo_state 闭环观测。

使用示例 ​

以下示例可完整编译运行（C++11；Linux g++ -std=c++11 需显式指定）。

cpp
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>

#include "cpp/interface/nrc_api.h" // 或只包含 "cpp/interface/nrc_interface.h"

namespace {

// ---- 现场参数（务必按实际控制器修改）----
const char* kRobotIp = "192.168.1.13";
const char* kCmdPort = "6001";   // 命令 / 状态通道：上下电、读坐标
const char* kServoPort = "7000"; // 伺服跟踪通道：servoJ 专用

constexpr int kDim = 7;          // servoJ 接口按 7 维处理：J1~J6 + 第 7 维
constexpr int kCycleMs = 10;     // 下发周期
constexpr int kSteps = 100;      // 总步数
constexpr double kStepDeg = 0.2; // 每周期第 1 轴增量（度）

void printVec(const char* tag, const std::vector<double>& v)
{
    std::cout << tag << " [";
    for (size_t i = 0; i < v.size(); ++i) {
        std::cout << (i ? ", " : "") << v[i];
    }
    std::cout << "]" << std::endl;
}

bool check(Result r, const char* name)
{
    std::cout << name << " -> " << static_cast<int>(r)
              << (r == SUCCESS ? " (SUCCESS)" : " (失败)") << std::endl;
    return r == SUCCESS;
}

// 等伺服进入运行状态（3=运行），最多等 timeoutMs
bool waitServoRunning(SOCKETFD fd, int timeoutMs)
{
    const int step = 100;
    for (int waited = 0; waited <= timeoutMs; waited += step) {
        int status = -1;
        if (check(get_servo_state(fd, status), "get_servo_state") && status == 3) {
            std::cout << "伺服已进入运行状态（3）" << std::endl;
            return true;
        }
        std::cout << "  当前伺服状态 = " << status << "，等待中…" << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(step));
    }
    return false;
}

// 无论从哪里退出，都要关闭伺服跟踪模式
struct ServoJGuard {
    SOCKETFD fd;
    bool armed;
    ~ServoJGuard()
    {
        if (armed) {
            std::cout << "关闭 servoJ: " << static_cast<int>(close_servoJ(fd)) << std::endl;
        }
    }
};

} // namespace

int main()
{
    // 1) 连接：命令通道 6001 + 伺服通道 7000（servoJ 系列只接受 7000 端口句柄）
    SOCKETFD fdCmd = connect_robot(kRobotIp, kCmdPort);
    SOCKETFD fdServo = connect_robot(kRobotIp, kServoPort);
    if (fdCmd < 0 || fdServo < 0) {
        std::cout << "连接失败: fd6001=" << fdCmd << " fd7000=" << fdServo << std::endl;
        return 1;
    }

    // 2) 清错 → 伺服就绪(1) → 上电（顺序由头文件注释规定，不可颠倒）
    check(clear_error(fdCmd), "clear_error");
    check(set_servo_state(fdCmd, 1), "set_servo_state(1)");
    check(set_servo_poweron(fdCmd), "set_servo_poweron");

    if (!waitServoRunning(fdCmd, 5000)) {
        std::cout << "伺服未进入运行状态，退出（未开启 servoJ）" << std::endl;
        disconnect_robot(fdCmd);
        disconnect_robot(fdServo);
        return 2;
    }

    // 3) 读取起始关节角（pos 长度固定为 7，函数内部会 resize）
    int coord = 0; // 0=关节坐标；注意是 int& 入参，不能直接传字面量
    std::vector<double> pos;
    if (!check(get_current_position(fdCmd, coord, pos), "get_current_position")) {
        disconnect_robot(fdCmd);
        disconnect_robot(fdServo);
        return 3;
    }
    printVec("起始关节角(度)", pos);

    // 4) 打开关节跟踪模式：速度/加速度/加加速度约束，必须 7 个元素
    //    实现按 [0]~[6] 硬索引，元素不足 7 个会越界（未定义行为）
    std::vector<double> vmax(kDim, 10.0);    // 度/秒
    std::vector<double> amax(kDim, 100.0);   // 度/秒^2
    std::vector<double> jmax(kDim, 1000.0);  // 度/秒^3
    if (!check(open_servoJ(fdServo, vmax, amax, jmax), "open_servoJ")) {
        disconnect_robot(fdCmd);
        disconnect_robot(fdServo);
        return 4;
    }
    ServoJGuard guard{fdServo, true};

    // 5) 周期下发目标关节角：以"上一次下发值"为基准累加，不要每周期重新读实际位置
    for (int i = 0; i < kSteps; ++i) {
        pos[0] += kStepDeg;
        check(set_servoJ_pos(fdServo, pos), "set_servoJ_pos");

        // 每 20 个周期看一次实际位置，用于观察跟随效果（6001 通道）
        if (i % 20 == 0) {
            std::vector<double> actual;
            int coordNow = 0;
            get_current_position(fdCmd, coordNow, actual);
            std::cout << "第 " << i << " 步 目标 J1=" << pos[0];
            if (actual.size() > 0) {
                std::cout << " 实际 J1=" << actual[0];
            }
            std::cout << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(kCycleMs));
    }

    // 6) 收尾：guard 析构时 close_servoJ；再加一点余量再断连
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
    disconnect_robot(fdCmd);
    disconnect_robot(fdServo);
    std::cout << "示例结束" << std::endl;
    return 0;
}

使用要点 ​

  • 向量必须是 7 个元素：vmax / amax / jmax / q 在 SDK 内部按下标 0~6 处理且不做边界检查，元素少于 7 个会越界（未定义行为）。统一用 std::vector<double>(7, ...) 构造。
  • 下发的是目标绝对关节角：增量请在本地累加（pos[0] += 0.2）后下发；每周期重新读实际位置再累加会把测量噪声叠进轨迹，运动容易抖。
  • 增量要和约束匹配：控制器按 vmax/amax/jmax 平滑下发点，下发速度快于约束允许速度时实际位置会滞后于目标。经验取法 每周期增量(°) ≤ vmax(°/s) × 周期(s)，例如 vmax = 10°/s、周期 10 ms 时每周期增量不超过 0.1°。
  • 固定周期在独立线程里发点：servoJ 是"喂点"模型，周期抖动会直接反映为运动不平滑；UI 线程里发点时尤其明显。
  • 必须收尾 close_servoJ：异常或提前 return 时也要保证调用（示例用 ServoJGuard 在析构时关闭），否则机器人会停在跟踪模式。
  • 该接口没有 _robot 变体：servoJ 系列固定作用于机器人 1。

常见问题 ​

现象可能原因处理
open_servoJ 返回 -2传入了 6001 端口句柄，或连接本身就失败确认 connect_robot(ip, "7000") 成功，并把 7000 句柄传给 servoJ 接口
返回 0 但机器人不动伺服未上电 / 未就绪；增量过小；没有连续下发用 get_servo_state 确认状态为 3；先小步长试跑；确认周期线程确实在发点
一动就报警或掉使能目标点超出软限位、单步过大、约束过小跟不上从当前位置起算、减小增量、把约束调整到合理值
运动抖动或一顿一顿周期不稳定；每周期重读实测位置再累加；上位机非实时独立线程固定周期；本地累加；缩短日志与 IO
程序崩溃且无报错向量少于 7 个元素导致越界一律用 std::vector<double>(7, ...)
断开连接后机器人仍在跟踪模式没有调用 close_servoJ加 RAII / 异常保护，断连前先关闭
编译报 'stop_servoJ' was not declared沿用了旧接口名改用 close_servoJ

说明 ​

  • 本页接口原型、参数与返回值以 SDK v2.0.4（cpp/interface/nrc_interface.h）为准；关闭接口名为 close_servoJ。
  • 控制器侧行为（如长时间未下发目标点时是否自动停止）请以真机实测结果为准。