控制机器人：

头部与腰部控制

控制路线推荐

头部与腰部控制推荐按以下优先级选择接口路线：

路线	接口	定位与使用条件
推荐	ROS 2 /robot/command	推荐通过 ROS2 服务 控制头部和腰部。服务类型为 rm_robot_interfaces/srv/StringCmd，请求和响应字段均为 string data。
受限	S100 TCP JSON（192.168.127.11:8080）	属于底层直连接口，普通二次开发不建议直接使用；确有底层集成需求时，请联系睿尔曼技术支持完成接口确认和安全评审。

服务类型与请求结构

ROS2 的服务类型为 rm_robot_interfaces/srv/StringCmd，请求与响应都仅包含一个 string data 字段。

请求字符串必须解析为"外层 wrapper + 内层 payload"两层对象：

• 外层 device 决定 rm_aloha_slave 使用哪条控制连接；
• 内层 device 由 S100 区分腰部和头部。

控制腰部时，外层 device 与 payload.device 均填写 2；控制头部时，两者均填写 3。两个值不一致时，命令可能无法正确路由或不生效。

部位编码与自由度

device	控制对象	状态反馈	示例安全范围
2	腰部（5 自由度）	/robot_slave/states 的 body	单关节最大相对 0.2°
3	头部（2 自由度）	/robot_slave/states 的 head	最大相对 3°

状态读取与新鲜度判断

• /robot_slave/states 的 position 为 0.001°，频率约 2 Hz；
• /mcap/body 的位置为 rad、速度为 rad/s，标称发布周期 5 ms。

说明

必须结合时间戳、位置变化与 ros2 topic hz 联合判断数据新鲜度。

movej 运动指令

头腰 movej payload 的现行形状如下。J* 必须替换为刚读回、通过驱动器限位和项目安全范围校验的整数目标；示例有意不放置历史姿态：

{
  "device": 3,
  "payload": {
    "command": "movej",
    "joint": ["J1", "J2"],
    "v": 1,
    "r": 0,
    "trajectory_connect": 0,
    "device": 3
  }
}

• 腰部运动将两处 device 改为 2，并使用五个 joint；
• v、r 与 trajectory_connect 是当前 wrapper 的必需字段；

停止指令

停止必须使用同一部位，并包含减速字段。

{
  "device": 3,
  "payload": {
    "command": "set_stop_teach",
    "device": 3,
    "v": 5,
    "r": 0
  }
}

• 腰部停止将两处 device 改为 2；

• 任何超时都表示结果未知：停止一次、读取新状态，不盲目重发旧目标。

驱动器限位查询

只读驱动器限位命令为 get_joint_drive_min_pos 与 get_joint_drive_max_pos，同样要求内外层 device 相同。

每次运动前必须重新读取，不能硬编码继承。

关节示教

关节示教属于持续运动，风险高于点到点 movej。

• teach_joint：关节序号，不是角度；
• direction：字符串，使用 "pos" 或 "neg" ，不是整数。

报文结构为 set_joint_teach + teach_joint + direction + v + device。

安全要求：示教开始后，必须在正常、超时、异常与连接断开的中显式发送 set_stop_teach；禁止依赖 TCP 断开自动停止。