3. 直接运动指令
本期介绍如何在示教模式下完成上电后,直接下发 MOVJ(关节运动)与 MOVL(直线运动)指令,并在指令执行期间等待运动完成。
直接运动指令不依赖作业文件,适合单点调试、点位验证和简单取放场景。运动前应先做可达性检测,运动后必须等待运行状态回到停止,不能仅凭指令返回值判断运动结束。
⚠️ 安全警告
- 运行前确认机器人周围无人员和障碍物
- 首次运行请使用低速
- 建议先在仿真环境中验证,再上真机
- 随时准备按下急停按钮
1、配置运动参数与点位
Demo 把速度、加减速和目标点集中定义在文件开头,点位同时保存「可达性检测用」和「运动用」两组数据:
# MOVJ关节运动速度,单位 % [0-100]
MOVJ_VELOCITY = 50
# MOVL直线运动速度,单位 mm/s [0-1000]
MOVL_VELOCITY = 500
# 机器人运动加速度
MOVE_ACC = 50
# 机器人运动减速度
MOVE_DEC = 50
# ===== 机器人点位统一管理 =====
POINTS = {
"POINT1": {
"check": [1, 1, 0, 0, 0, 0, 0, 100, 300, 500, 3.1415, 0, 0, 0],
"pos": [100, 300, 500, 3.1415, 0, 0, 0]
},
"POINT2": {
"check": [1, 1, 0, 0, 0, 0, 0, 500, 200, 500, 3.1415, 0, 0, 0],
"pos": [500, 200, 500, 3.1415, 0, 0, 0]
}
}check 与 pos 的区别在于前 7 位:
| 字段 | 长度 | 含义 |
|---|---|---|
check | 14 | 第 1、2 位固定为 1, 1,第 3~7 位为姿态/轴配置标志,后 7 位为笛卡尔位姿 x, y, z, rx, ry, rz, 附加轴 |
pos | 7 | 纯笛卡尔位姿 x, y, z, rx, ry, rz, 附加轴,直接用于 MoveCmd.targetPosValue |
可达性检测接口 get_pos_reachable 要求的点位格式与运动接口不同,因此两者分开保存,避免调用时临时拼接出错。
单位与量纲
MOVJ_VELOCITY = 50 表示关节速度百分比,MOVL_VELOCITY = 500 表示末端速度 mm/s。两者量纲不同,不能互相比较或套用同一数值。
2、构造运动指令
robot_movej 和 robot_movel 都需要一个 MoveCmd 对象,通过统一函数构造:
def create_move_cmd(pos, velocity):
cmd = nrc_interface.MoveCmd()
for i in range(len(pos)):
cmd.targetPosValue[i] = pos[i]
cmd.coord = 1 # 1:笛卡尔坐标系
cmd.velocity = velocity
cmd.acc = MOVE_ACC
cmd.dec = MOVE_DEC
cmd.isSync = True # 同步执行,指令返回后仍需等待运动完成
return cmdcoord 的取值含义:0 表示关节点位,1 表示笛卡尔点位。Demo 中目标点是笛卡尔位姿,因此固定传 1。
3、可达性检测
在运动之前先调用 get_pos_reachable 判断目标点是否可达,避免下发不可达点位导致运动异常:
def check_reachable(socketFd, pos, movetype):
result = False
for i in range(REACHABLE_RETRY_COUNT):
ret = nrc_interface.get_pos_reachable(socketFd, pos, movetype, result)
if ret[0] != 0:
print(f"第{i+1}次可达性检测接口失败,返回值:{ret}")
if i < REACHABLE_RETRY_COUNT - 1:
time.sleep(WAIT_REACHABLE_RETRY)
continue
raise Exception("连续多次可达性检测接口失败")
if ret[1] is False:
raise Exception(f"{movetype}目标点不可达")
print(f"{movetype}目标点可达")
return注意 ret[1] is False 不能写成 not ret[1]:当接口返回 None 或非布尔值时,not 会误判为不可达。
4、下发运动指令
def movej(socketFd, pos):
cmd = create_move_cmd(pos, MOVJ_VELOCITY)
check_result(nrc_interface.robot_movej(socketFd, cmd), "MOVJ运动")
print("MOVJ运动指令发送成功")
def movel(socketFd, pos):
cmd = create_move_cmd(pos, MOVL_VELOCITY)
check_result(nrc_interface.robot_movel(socketFd, cmd), "MOVL运动")
print("MOVL运动指令发送成功")接口返回 0 只代表指令下发成功,不代表运动已经结束。
5、等待运动完成
运动完成判据是「运行状态连续多次为停止(0)」。单次查询到停止可能是状态刷新延迟,因此需要连续确认:
def wait_move_done(socketFd, timeout=MOVE_TIMEOUT):
start = time.time()
stop_count = 0
running_state_fail_count = 0
while True:
status = nrc_interface.get_robot_running_state(socketFd, 0)
if status[0] != 0:
running_state_fail_count += 1
print(f"第{running_state_fail_count}次获取机器人运行状态失败,返回值:{status}")
if running_state_fail_count >= RUNNING_STATE_RETRY_COUNT:
raise Exception("连续多次获取机器人运行状态失败")
time.sleep(WAIT_RUNNING_STATE_RETRY)
continue
# 查询成功后清零失败次数,避免偶发通信失败累计到后续查询
running_state_fail_count = 0
print("机器人运行状态:", status)
if status[1] == 0:
stop_count += 1
print(f"机器人停止状态确认:{stop_count}/{RUNNING_STATE_STOP_CONFIRM_COUNT}")
if stop_count >= RUNNING_STATE_STOP_CONFIRM_COUNT:
print("连续确认机器人停止,运动完成")
return
else:
# 重新检测到运行或暂停状态时,之前的停止状态不再连续
stop_count = 0
if time.time() - start > timeout:
raise TimeoutError("机器人运动超时")
time.sleep(WAIT_QUERY_INTERVAL)运行状态取值:0 停止、1 暂停、2 运行。
6、主流程与启动等待时间
def main():
socketFd = None
try:
socketFd = connect_robot()
print("连接成功 socketFd =", socketFd)
ensure_teach_mode(socketFd)
state = get_servo_status(socketFd)
power_on(socketFd, state)
check_reachable(socketFd, POINTS["POINT1"]["check"], "MOVJ")
print("POINT1点检测通过")
movej(socketFd, POINTS["POINT1"]["pos"])
time.sleep(WAIT_START_MOVJ)
wait_move_done(socketFd)
time.sleep(WAIT_POINT_REACH)
check_reachable(socketFd, POINTS["POINT2"]["check"], "MOVL")
print("POINT2点检测通过")
movel(socketFd, POINTS["POINT2"]["pos"])
time.sleep(WAIT_START_MOVL)
wait_move_done(socketFd)
print("Demo执行成功")
...WAIT_START_MOVJ = 0.2、WAIT_START_MOVL = 1.2 是两类指令下发后进入执行状态所需的初始化时间差:MOVL 的轨迹规划耗时更长,若立即查询运行状态,可能仍读到「停止」而误判运动已完成。这是本 Demo 最容易被忽略的两个参数。
| 参数 | 值 | 说明 |
|---|---|---|
WAIT_START_MOVJ | 0.2 | MOVJ 下发后等待进入运行状态 |
WAIT_START_MOVL | 1.2 | MOVL 下发后等待进入运行状态,明显长于 MOVJ |
WAIT_POINT_REACH | 2 | 到达点位后稳定等待,再检测下一点 |
WAIT_QUERY_INTERVAL | 0.5 | 运行状态轮询间隔 |
MOVE_TIMEOUT | 30 | 单次运动超时时间(秒) |
7、运行与预期输出
把 ip 改为现场控制器地址(port 保持字符串 "6001"),在示教器确认机器人已上电且处于安全位置后运行:
python 03_movj_wait.py预期输出依次出现:连接成功、模式确认、伺服状态、上电成功、MOVJ目标点可达、MOVJ运动指令发送成功、停止状态确认计数、MOVL目标点可达、MOVL运动指令发送成功、Demo执行成功、连接关闭。
8、常见问题
可达性检测一直失败
检查 check 数组长度是否为 14 且前两位为 1, 1。该接口格式与 pos 不同,混用会被判为参数错误。
运动指令返回 0 但机器人不动
依次确认:是否已在示教模式完成上电;是否存在报警未清;目标点是否在软限位内。
wait_move_done 立刻返回
WAIT_START_MOVJ/WAIT_START_MOVL 设置过小,或 RUNNING_STATE_STOP_CONFIRM_COUNT 被改为 1。MOVL 场景下至少保留 1 秒启动等待。
运行几秒后抛 TimeoutError
MOVE_TIMEOUT = 30 不足以覆盖本次行程,按实际轨迹长度调大。
下一步
需要按点位序列连续运行、可暂停可继续时,改用作业文件执行,参见 5. 新建并执行作业文件。
完整可运行脚本见 devkit:04-上位机/Python/RTL-24.03(当前)/04-示例/基础示例/03-直接运动指令/03_movj_wait.py。