头部、手臂、腰部及手部控制接口

本页说明 Roban2 当前已公开的部位控制接口。手臂、头部和腰部优先使用 lejusdk-vr 的部位轨迹接口；双手使用 lejusdk-lowlevel 的 HandCmd。RobotCmd 为高风险底层全关节接口，仅在确实需要自定义关节控制时使用。

部位控制模式

1. 功能描述

设置手臂、头部或腰部的控制权归属。手臂和腰部在接收外部轨迹前需切换到 kExternal；使用结束后应恢复到业务所需模式。

2. 所属 SDK 和头文件

• SDK：lejusdk-vr
• 头文件：<lejusdk-vr/lejusdk_vr.h>

3. 请求或消息类型

bool setArmMode(leju::vr::ControlMode mode, int timeout_ms = 3000);
bool setHeadMode(leju::vr::ControlMode mode, int timeout_ms = 3000);
bool setWaistMode(leju::vr::ControlMode mode, int timeout_ms = 3000);

4. 字段说明

ControlMode	数值	说明
kKeepPose	0	保持当前姿态
kAuto	1	RL 控制器自动控制
kExternal	2	接受外部部位指令

timeout_ms 是 DDS-RPC 等待超时，默认 3000 ms。

5. 返回值

服务端接受并完成设置返回 true；服务不可用、超时或设置失败返回 false。

6. 使用前提

• RobanVRAPI 已初始化，控制器管理服务正在运行。
• 切换前确认该部位不在执行与外部控制冲突的动作。
• 可通过 getControllerState() 确认最终模式。

7. C++ 示例

leju::vr::RobanVRAPI vr(leju::RobotVersion::from_env());
if (!vr.initialize()) return 1;
if (!vr.setArmMode(leju::vr::ControlMode::kExternal)) return 2;
if (!vr.setWaistMode(leju::vr::ControlMode::kExternal)) return 3;

8. 对应仓库示例

• Roban 手臂：src/lejusdk/examples/src/lejusdk-vr/07_roban_arm.cpp
• 腰部流程：src/lejusdk/examples/src/lejusdk-vr/06_waist_control.cpp
• 交互式模式设置：src/lejusdk/examples/src/lejusdk-vr/08_controller_cli.cpp

06_waist_control.cpp 和 08_controller_cli.cpp 使用通用的 KuavoVRAPI 示范流程；Roban2 二次开发应使用 RobanVRAPI。

手臂关节控制

1. 功能描述

publishArmJointCmd() 发布 Roban2 双臂 8 维关节轨迹点。subscribeArmJointCmd() 可用于监听同一轨迹数据流。

2. 所属 SDK 和头文件

• SDK：lejusdk-vr
• 头文件：<lejusdk-vr/lejusdk_vr.h>

3. 请求或消息类型

bool publishArmJointCmd(const leju::vr::JointTrajectoryPoint& cmd);
void subscribeArmJointCmd(leju::vr::ArmTrajectoryCallback callback);

4. 字段说明

JointTrajectoryPoint 包含：

字段	类型	单位
q	std::vector<double>	目标位置，rad
v	std::vector<double>	目标速度，rad/s
acc	std::vector<double>	目标加速度，rad/s²

Roban2 手臂 q 顺序为：

[zarm_l1, zarm_l2, zarm_l3, zarm_l4,
 zarm_r1, zarm_r2, zarm_r3, zarm_r4]

即先左臂 4 维，再右臂 4 维。v 和 acc 按相同顺序。

5. 返回值

• publishArmJointCmd()：发布成功返回 true，失败返回 false。
• subscribeArmJointCmd()：无返回值。

6. 使用前提

• 手臂已通过 setArmMode(kExternal) 切换为外部控制。
• q 使用 8 维 Roban2 顺序；建议 v 和 acc 也提供 8 维数据。
• 指令应使用平滑轨迹连续发布，不要从当前角度跳变到大幅度目标。

7. C++ 示例

using leju::vr::ControlMode;
using leju::vr::JointTrajectoryPoint;

if (!vr.setArmMode(ControlMode::kExternal)) return 1;

JointTrajectoryPoint cmd;
cmd.q = {0.0, 0.10, 0.0, -0.20,
         0.0, -0.10, 0.0, -0.20};
cmd.v.assign(8, 0.0);
cmd.acc.assign(8, 0.0);
if (!vr.publishArmJointCmd(cmd)) return 2;

8. 对应仓库示例

• src/lejusdk/examples/src/lejusdk-vr/07_roban_arm.cpp

该文件明确使用 RobanVRAPI 和 Roban 8 维手臂顺序，可直接作为 Roban2 流程参考。

头部关节控制

1. 功能描述

publishHeadJointCmd() 发布 Roban2 头部 2 维关节轨迹；subscribeHeadJointCmd() 订阅头部轨迹。

2. 所属 SDK 和头文件

• SDK：lejusdk-vr
• 头文件：<lejusdk-vr/lejusdk_vr.h>

3. 请求或消息类型

bool publishHeadJointCmd(const leju::vr::JointTrajectoryPoint& cmd);
void subscribeHeadJointCmd(leju::vr::HeadTrajectoryCallback callback);

4. 字段说明

JointTrajectoryPoint 字段与手臂接口一致。Roban2 当前控制器消费顺序为：

下标	关节	语义
q[0]	zhead_1_joint	yaw，偏航
q[1]	zhead_2_joint	pitch，俯仰，正值低头

5. 返回值

• publishHeadJointCmd()：发布成功返回 true。
• subscribeHeadJointCmd()：无返回值。

6. 使用前提

• RobanVRAPI 已初始化，控制链路正在运行。
• 当前控制器对头部轨迹直接透传，仓库示例未要求先切换头部模式。如业务使用了头部模式管理，应通过 getControllerState() 确认实际模式。
• 以 Roban2 控制器的 yaw, pitch 顺序为准。通用示例 05_head_control.cpp 中的注释顺序不适用于当前 Roban2 消费端。

7. C++ 示例

leju::vr::JointTrajectoryPoint head;
head.q = {0.15, 0.10};  // yaw 左转，pitch 低头
head.v = {0.0, 0.0};
head.acc = {0.0, 0.0};
if (!vr.publishHeadJointCmd(head)) return 1;

8. 对应仓库示例

• SDK 流程：src/lejusdk/examples/src/lejusdk-vr/05_head_control.cpp
• Roban2 实际消费顺序：src/leju-controllers/leju-rl-controller/src/controllers/controller_manager.cpp

05_head_control.cpp 使用 KuavoVRAPI 且关节注释顺序与 Roban2 控制器不同，只参考发布流程，不复制其机型类型和数组语义。

腰部关节控制

1. 功能描述

publishWaistJointCmd() 发布 Roban2 腰部 1 维偏航轨迹；subscribeWaistJointCmd() 订阅腰部轨迹。

2. 所属 SDK 和头文件

• SDK：lejusdk-vr
• 头文件：<lejusdk-vr/lejusdk_vr.h>

3. 请求或消息类型

bool publishWaistJointCmd(const leju::vr::JointTrajectoryPoint& cmd);
void subscribeWaistJointCmd(leju::vr::WaistTrajectoryCallback callback);

4. 字段说明

q[0] 是腰部 yaw 目标角，单位 rad，正值向左转。v[0] 和 acc[0] 分别是目标速度和加速度。

5. 返回值

• publishWaistJointCmd()：发布成功返回 true。
• subscribeWaistJointCmd()：无返回值。

6. 使用前提

• 腰部已通过 setWaistMode(kExternal) 切换到外部控制。
• 指令维度为 1，且目标在实机可行范围内。
• 使用结束时将腰部平滑返回期望位置，然后恢复 kAuto 或业务要求的模式。

7. C++ 示例

if (!vr.setWaistMode(leju::vr::ControlMode::kExternal)) return 1;

leju::vr::JointTrajectoryPoint waist;
waist.q = {0.10};
waist.v = {0.0};
waist.acc = {0.0};
if (!vr.publishWaistJointCmd(waist)) return 2;

8. 对应仓库示例

• src/lejusdk/examples/src/lejusdk-vr/06_waist_control.cpp

该示例的腰部维度和流程可参考；Roban2 程序应把 KuavoVRAPI 替换为 RobanVRAPI。

双手位置控制

1. 功能描述

publishHandCmd() 发布双手 12 维手指位置目标。

2. 所属 SDK 和头文件

• SDK：lejusdk-lowlevel
• 头文件：<lejusdk-lowlevel/leju_sdk.h>

3. 请求或消息类型

bool publishHandCmd(const leju::HandCmd& cmd);

4. 字段说明

字段	类型	说明
position	std::vector<double>	固定 12 维，前 6 维左手，后 6 维右手，取值 [0, 100]
timestamp	double	时间戳，单位 s

HandCmd::isValid() 只检查数组是否为 12 维；应用程序仍需确保每个值位于 [0, 100]。公开头文件未定义六个手指通道的具体手指名称，本文不做额外假设。

5. 返回值

发布成功返回 true，失败返回 false。

6. 使用前提

• 低层 SDK 已初始化，手部指令消费端已启动。
• 确认设备实际安装的末端类型支持此 12 维位置指令。
• 首次调试使用小幅度变化，避免夹持人员或物体。

7. C++ 示例

leju::HandCmd hand;
hand.position = {10, 10, 10, 10, 10, 10,
                 10, 10, 10, 10, 10, 10};
if (!hand.isValid() || !robot.publishHandCmd(hand)) return 1;

8. 对应仓库示例

当前 src/lejusdk/examples/ 未提供独立的 publishHandCmd() 用户示例。接口和 12 维字段定义以以下公开头文件为准：

• installed/include/lejusdk-lowlevel/robot_api/robot_base.h
• installed/include/lejusdk-lowlevel/data_types.h

底层全关节控制

1. 功能描述

publishRobotCmd() 向全部关节发布位置、速度、力矩、增益和电机模式。该接口绕过部位级轨迹抽象，仅适用于掌握整机关节顺序和电机控制律的高级开发。

2. 所属 SDK 和头文件

• SDK：lejusdk-lowlevel
• 头文件：<lejusdk-lowlevel/leju_sdk.h>

3. 请求或消息类型

bool publishRobotCmd(const leju::RobotCmd& cmd);

4. 字段说明

字段	类型	说明
q	std::vector<double>	目标位置，rad
v	std::vector<double>	目标速度，rad/s
tau	std::vector<double>	前馈力矩，N·m
kp	std::vector<double>	位置增益，N·m/rad
kd	std::vector<double>	速度增益，N·m·s/rad
modes	std::vector<uint8_t>	各关节 MotorControlMode
timestamp	double	时间戳，s

模式为 CST=0 力矩、CSV=1 速度、CSP=2 位置。CST 模式下 kp 和 kd 必须为 0。所有数组维度必须与 getMotorNumber() 一致，顺序与 getMotorNames() 一致。

5. 返回值

发布成功返回 true，失败返回 false。返回 true 只表示 DDS 发布成功；指令只有在硬件状态为 READY_OK 时才会生效。

6. 使用前提

• 已订阅硬件状态并等待 READY_OK。
• 所有数组维度一致，cmd.isValid() 返回 true。
• 指令频率、关节限位、增益和力矩限制已经过所用硬件配置验证。
• 实机有物理保护、急停和现场监护。

7. C++ 示例

const auto count = robot.getMotorNumber();
leju::RobotCmd cmd(count);
for (std::size_t i = 0; i < count; ++i) {
  cmd.q[i] = 0.0;
  cmd.v[i] = 0.0;
  cmd.tau[i] = 0.0;
  cmd.kp[i] = 0.0;  // 具体增益不在文档中臆造
  cmd.kd[i] = 0.0;
  cmd.modes[i] = static_cast<uint8_t>(leju::MotorControlMode::CSP);
}

if (!cmd.isValid()) return 1;
// 只有在确认 READY_OK 且填入实机验证过的目标/增益后才发布。

8. 对应仓库示例

• 完整发布流程：src/lejusdk/examples/src/joint_cmd_publisher.cpp
• 综合控制流程：src/lejusdk/examples/src/kuavo_control_example.cpp

请先阅读仓库示例中的硬件就绪判断、关节顺序和定时循环，不应只复制单次 publishRobotCmd() 调用。