4. 直接运动指令
本期将介绍如何通过 SDK 依次发送 MOVJ 和 MOVL 直接运动指令，并在每段运动开始前执行目标点可达性预检，在指令发送后等待机器人运动完成。
MOVJ 和 MOVL 的运动方式不同：
- MOVJ： 以关节运动方式到达目标点，当前 Demo 使用关节坐标数据
- MOVL： 控制机器人末端沿直线路径到达目标点，当前 Demo 使用直角坐标数据
本 Demo 的执行顺序为：先检查目标点人工确认标志，再执行 MOVJ 可达性预检和运动；确认 MOVJ 完全结束后，重新执行 MOVL 可达性预检并发送 MOVL 指令，最后等待第二段运动完成。
⚠️ 安全警告
以下代码会向控制器发送真实运动指令，并导致机器人实际运动。执行前必须确认：
- 人员安全： 机器人工作空间及规划路径内无人员、工装干涉或障碍物
- 目标点来源： MOVJ 和 MOVL 目标点均来自当前机器人实际示教，禁止直接使用其他机器人或其他现场的坐标
- 坐标配置： 坐标类型、工具坐标系、用户坐标系和机器人构型与示教目标点一致
- 伺服与模式： 伺服已经上电，控制器处于允许 SDK 下发运动指令的模式
- 速度限制： 首次调试使用较低速度和较缓加减速度，并结合机器人负载逐步调整
- 急停可用： 急停按钮处于可用状态，并在操作人员可触及范围内
- 路径安全： 可达性预检只能判断运动学是否可达，不能代替碰撞检测和真实路径安全检查
建议先在仿真环境中验证目标点、坐标系和运动顺序，再在空载低速条件下进行真机测试。
1、配置控制器、目标点和运动参数
首先配置控制器 IP 地址和 SDK 服务端口。程序只有连接到正确的控制器，才能对目标点执行预检并发送运动指令。
cpp
/**
* @name 用户必须根据运行环境修改
* 以下配置决定 SDK 能否连接到实际控制器，运行前必须核对。
* @{
*/
const std::string robot_ip = "192.168.3.243"; ///< 实际控制器的 IP 地址。
const std::string robot_port = "6001"; ///< 实际控制器的 SDK 服务端口，必须与控制器配置一致。
/** @} */MOVJ 和 MOVL 目标点必须来自当前机器人实际示教。源码默认使用全零数据作为占位值，并将 target_positions_confirmed 设置为 false，防止程序直接使用未经验证的目标点驱动机器人。
cpp
/**
* @name 客户需根据本 Demo 修改
* 目标点必须来自当前机器人实际示教；确认点位和运行环境安全后，才可修改确认标志。
* @{
*/
const std::array<double, 7> movej_target = {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
}; ///< MOVJ 实际示教关节目标点，禁止使用未确认点位。
const std::array<double, 7> movel_target = {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
}; ///< MOVL 实际示教直角目标点，坐标格式必须匹配机器人型号。
constexpr bool target_positions_confirmed = false; ///< 两个目标点均完成示教和安全确认后才可设为 true。
/** @} */运动速度、加速度和减速度应结合机器人型号、负载和现场条件设置。首次调试时应保留较低参数，再根据实际运行情况逐步调整。
cpp
/**
* @name 建议客户修改
* 首次调试建议采用较低速度和较缓加减速度，并结合机器人负载及现场条件逐步调整。
* @{
*/
constexpr double movej_velocity = 10.0; ///< MOVJ 关节运动速度。
constexpr double movel_velocity = 20.0; ///< MOVL 直线运动速度。
constexpr double motion_acceleration = 20.0; ///< 运动加速度。
constexpr double motion_deceleration = 20.0; ///< 运动减速度。
/** @} */等待参数用于限制单次运动的最长等待时间、控制运行状态查询频率，并处理短暂通信异常和极短运动状态难以捕获的问题。
cpp
/**
* @name 可修改也可保留默认值
* 以下配置控制运动完成等待、状态轮询和通信容错，默认值适用于一般演示场景。
* @{
*/
constexpr int motion_timeout_seconds = 30; ///< 单次运动完成等待的总超时时间，单位为秒。
constexpr int poll_interval_ms = 100; ///< 机器人运行状态的轮询间隔，单位为毫秒。
constexpr int max_state_query_failures = 3; ///< 运行状态查询允许的最大连续失败次数。
constexpr int stopped_confirmation_count = 5; ///< 未观察到运行状态时，判定运动完成所需的连续停止状态次数。
/** @} */2、封装运动指令构造函数
MOVJ 和 MOVL 都使用 MoveCmd 结构传递目标点、坐标类型、速度、加减速度、平滑参数和坐标系编号。两种指令的大部分公共参数相同，只有目标点、坐标类型和速度不同。
cpp
/**
* @brief 使用统一的公共参数构造 MOVJ 或 MOVL 运动指令。
* @param[in] target 七维运动目标值。
* @param[in] coord 目标点坐标类型；当前 Demo 使用 0 表示关节坐标，1 表示直角坐标。
* @param[in] velocity 本次运动使用的速度。
* @return 填充完成的运动指令参数。
*/
MoveCmd make_move_command(
const std::array<double, 7>& target,
int coord,
double velocity)
{
MoveCmd command;
command.targetPosType = PosType::data;
std::copy(target.begin(), target.end(), command.targetPosValue.begin());
command.coord = coord;
command.velocity = velocity;
command.acc = motion_acceleration;
command.dec = motion_deceleration;
command.pl = 0; // 关闭与下一段轨迹的平滑过渡，便于准确判断本段结束。
command.toolNum = 0; // 必须与示教目标点使用的工具坐标系编号一致。
command.userNum = 0; // 必须与示教目标点使用的用户坐标系编号一致。
command.configuration = 0; // 必须按机器人型号填写目标点构型。
return command;
}其中 pl = 0 表示本 Demo 不考虑与下一段运动之间的平滑过渡。程序会等待当前运动完全结束后再发送下一条指令，因此能够明确区分 MOVJ 和 MOVL 两段运动。
toolNum、userNum 和 configuration 不能仅因为示例值为 0 就直接保留。实际使用时，必须与目标点示教时采用的工具坐标系、用户坐标系和机器人构型一致。
3、封装可达性位置数据转换函数
运动指令使用 MoveCmd 保存目标数据，而 get_pos_reachable 接口要求传入长度为 14 的位置容器。程序需要将运动指令转换为可达性接口规定的数据格式。
cpp
/**
* @brief 将运动指令转换为可达性接口要求的 14 位位置数据。
* @param[in] command 待执行的运动指令参数。
* @return 用于可达性预检的位置数据。
*
* @details
* 转换结果与实际下发指令使用同一目标数据，避免预检点位与真实运动点位不一致。
*/
std::vector<double> make_reachability_position(const MoveCmd& command)
{
// 前 7 位描述坐标系、角度制和构型，后 7 位保存实际目标坐标。
std::vector<double> position(14, 0.0);
position[0] = static_cast<double>(command.coord);
position[1] = 0.0; // 当前目标使用角度制；使用弧度制时必须与实际数据保持一致。
position[2] = static_cast<double>(command.configuration);
position[3] = static_cast<double>(command.toolNum);
position[4] = static_cast<double>(command.userNum);
std::copy_n(command.targetPosValue.begin(), 7, position.begin() + 7);
return position;
}转换后的 14 位位置数据含义如下：
- 第 0 位：目标点坐标类型
- 第 1 位：角度制或弧度制标志，当前 Demo 使用角度制
- 第 2 位：机器人构型
- 第 3 位：工具坐标系编号
- 第 4 位：用户坐标系编号
- 第 5～6 位：备用字段，保持为 0
- 第 7～13 位：七维目标位置数据
4、封装目标点可达性预检函数
在向机器人发送真实运动指令之前，程序先调用 get_pos_reachable 检查目标点在运动学上是否可达。只有 SDK 调用成功且接口返回目标点可达时，才允许继续发送运动指令。
单独封装 precheck_reachability，是为了统一 MOVJ 和 MOVL 的预检流程、错误处理和结果输出。主程序不需要重复编写位置转换、SDK 返回值判断和可达结果判断，也能保证预检失败时立即取消对应运动。
cpp
/**
* @brief 在发送真实运动指令前检查目标点是否可达。
* @param[in] fd 控制器连接句柄。
* @param[in] command 待预检的运动指令参数。
* @param[in] moveType 运动类型名称，用于调用接口和输出提示。
* @retval true SDK 调用成功且目标点可达。
* @retval false SDK 调用失败或目标点不可达。
*/
bool precheck_reachability(
SOCKETFD fd,
const MoveCmd& command,
const std::string& moveType)
{
bool reachable = false;
const Result result = get_pos_reachable(
fd,
make_reachability_position(command),
moveType,
reachable);
if (result != SUCCESS)
{
std::cerr << moveType << " 可达性预检调用失败，错误码："
<< static_cast<int>(result) << std::endl;
return false;
}
if (!reachable)
{
std::cerr << moveType << " 目标点不可达，已取消运动指令" << std::endl;
return false;
}
std::cout << moveType << " 目标点可达性预检通过" << std::endl;
return true;
}需要特别注意，可达性预检通过只代表控制器认为目标点在运动学上可达，并不代表运动路径一定不会发生碰撞。工具、工件、外围设备、机器人本体和奇异位形等风险仍需通过仿真和现场安全验证进行确认。
5、封装等待运动完成函数
robot_movej 或 robot_movel 返回 SUCCESS，只表示运动指令已经成功发送，不代表机器人已经到达目标点。程序需要持续调用 get_robot_running_state 查询运行状态，直到确认本段运动结束。
运行状态的含义如下：
- 状态 0：机器人停止
- 状态 1：机器人暂停
- 状态 2：机器人正在运行
cpp
/**
* @brief 等待当前运动完成，并统一处理暂停、查询失败、未知状态和超时。
* @param[in] fd 控制器连接句柄。
* @param[in] timeout 本次等待允许占用的最长时间。
* @retval true 已确认机器人运动完成。
* @retval false 状态查询连续失败、返回未知状态或等待超时。
*/
bool wait_for_motion_complete(
SOCKETFD fd,
std::chrono::seconds timeout = std::chrono::seconds(motion_timeout_seconds))
{
using clock = std::chrono::steady_clock;
const auto deadline = clock::now() + timeout; // 使用绝对截止时间限制整个等待过程。
bool runningObserved = false; // 是否观察到运行状态 2。
bool pauseReported = false; // 暂停期间只提示一次。
int stoppedCount = 0; // 连续停止状态的确认次数。
int consecutiveFailures = 0; // 运行状态连续查询失败次数。
while (clock::now() < deadline)
{
int runningState = 0;
const Result result = get_robot_running_state(fd, runningState);
if (result != SUCCESS)
{
++consecutiveFailures;
std::cerr << "获取机器人运行状态失败，第 "
<< consecutiveFailures << "/" << max_state_query_failures
<< " 次，错误码：" << static_cast<int>(result) << std::endl;
if (consecutiveFailures >= max_state_query_failures)
{
std::cerr << "连续获取机器人运行状态失败，停止等待" << std::endl;
return false;
}
std::this_thread::sleep_for(
std::chrono::milliseconds(poll_interval_ms));
continue;
}
consecutiveFailures = 0; // 查询成功后清除连续失败计数。
switch (runningState)
{
case 2:
// 已确认运动开始，后续读取到停止状态即可判定本段运动结束。
runningObserved = true;
pauseReported = false;
stoppedCount = 0;
break;
case 1:
// 暂停不等于完成，继续等待恢复；总等待时间仍受 timeout 限制。
stoppedCount = 0;
if (!pauseReported)
{
std::cout << "机器人运动已暂停，继续等待恢复" << std::endl;
pauseReported = true;
}
break;
case 0:
// 已观察到运行时，状态 0 表示结束；否则连续确认以兼容极短运动。
++stoppedCount;
if (runningObserved || stoppedCount >= stopped_confirmation_count)
{
std::cout << "机器人运动完成" << std::endl;
return true;
}
break;
default:
std::cerr << "机器人返回未知运行状态：" << runningState << std::endl;
return false;
}
std::this_thread::sleep_for(
std::chrono::milliseconds(poll_interval_ms));
}
std::cerr << "等待机器人运动完成超时" << std::endl;
return false;
}runningObserved 用于记录程序是否实际观察到机器人进入运行状态。对于正常运动，一旦先观察到状态 2，之后读取到状态 0 就可以确认运动结束。
某些运动时间很短，程序可能因为轮询间隔而没有捕获到状态 2。此时通过 stopped_confirmation_count 连续确认多次状态 0，避免程序一直等待，也降低单次瞬时状态造成误判的概率。
6、主程序依次执行 MOVJ 和 MOVL
主函数首先检查 target_positions_confirmed。确认标志为 false 时，程序会在连接控制器和发送运动指令之前直接退出，避免全零占位点或未经验证的目标点驱动机器人。
连接成功后，程序分别为 MOVJ 和 MOVL 构造运动指令。每条指令都必须先通过可达性预检，再检查运动接口的 SDK 返回值，并等待当前运动完全结束。
cpp
#include <algorithm>
#include <array>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>
#include <cpp_interface/nrc_interface.h>
#include "../demo_utils.h"
/**
* @brief Demo 程序入口：依次执行 MOVJ、MOVL，并等待每段运动完成。
* @return 全部预检、指令发送和运动等待均成功时返回 0，否则返回 1。
*/
int main()
{
demo::enable_console_utf8();
// 在连接和下发指令前检查人工确认标志，避免使用占位点或未经验证的点位。
if (!target_positions_confirmed)
{
std::cerr << "目标点尚未确认：请填写实际示教点，并将 "
<< "target_positions_confirmed 设置为 true" << std::endl;
return 1;
}
SOCKETFD fd = connect_robot(robot_ip, robot_port);
if (fd <= 0)
{
std::cerr << "控制器连接失败" << std::endl;
return 1;
}
std::cout << "控制器连接成功" << std::endl;
// MOVJ 使用关节坐标（coord = 0），仅在可达性预检通过后发送指令。
MoveCmd movejCommand = make_move_command(movej_target, 0, movej_velocity);
if (!precheck_reachability(fd, movejCommand, "MOVJ"))
{
return 1;
}
Result result = robot_movej(fd, movejCommand);
if (result != SUCCESS)
{
std::cerr << "发送 MOVJ 指令失败，错误码："
<< static_cast<int>(result) << std::endl;
return 1;
}
std::cout << "MOVJ 指令发送成功，等待运动完成" << std::endl;
// 等待 MOVJ 完全结束后再执行 MOVL，避免两条指令同时占用机器人。
if (!wait_for_motion_complete(fd))
{
return 1;
}
// MOVL 使用直角坐标（coord = 1），并基于当前实际位置重新执行预检。
MoveCmd movelCommand = make_move_command(movel_target, 1, movel_velocity);
if (!precheck_reachability(fd, movelCommand, "MOVL"))
{
return 1;
}
result = robot_movel(fd, movelCommand);
if (result != SUCCESS)
{
std::cerr << "发送 MOVL 指令失败，错误码："
<< static_cast<int>(result) << std::endl;
return 1;
}
std::cout << "MOVL 指令发送成功，等待运动完成" << std::endl;
// 将最后一次等待结果作为退出状态，便于外部脚本判断本次 Demo 是否成功。
return wait_for_motion_complete(fd) ? 0 : 1;
}