头部及手部控制
KUAVO Humanoid SDK 提供了全面的头部和手部控制接口，支持机器人头部运动控制、手臂运动控制以及不同类型的机械手（如 LejuClaw 和 DexterousHand）操作。本文档详细介绍这些控制接口的使用方法。
接口说明
头部控制接口
KuavoRobotHead
KuavoRobotHead 类提供了控制机器人头部运动的接口。
主要接口描述如下：
| 函数名 | control_head |
|---|---|
| 函数原型 | bool control_head(float yaw, float pitch) |
| 功能概述 | 控制机器人头部的偏航角(yaw)和俯仰角(pitch)。 |
| 参数 | yaw: 头部水平转动角度，范围[-1.396, 1.396]弧度（约-80°到80°） pitch: 头部俯仰角度，范围[-0.436, 0.436]弧度（约-25°到25°） |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 角度单位为弧度，超出范围会自动限制在有效范围内 |
手臂控制接口
KuavoRobotArm
KuavoRobotArm 类提供了控制机器人手臂的接口。
主要接口描述如下：
| 函数名 | control_arm_joint_positions |
|---|---|
| 函数原型 | bool control_arm_joint_positions(list joint_positions) |
| 功能概述 | 控制机器人手臂关节位置。 |
| 参数 | joint_positions: 目标关节位置列表，单位为弧度，长度为手臂关节数量（14个关节） |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 1. 关节角度范围必须为[-π, π]弧度 2. 机器人必须处于站立状态 3. 手臂必须处于外部控制模式 |
| 函数名 | arm_fk |
|---|---|
| 函数原型 | Tuple[KuavoPose, KuavoPose] arm_fk(list q) |
| 功能概述 | 计算机器人手臂的正向运动学。 |
| 参数 | q: 关节位置列表，单位为弧度 |
| 返回值 | 左右手臂末端执行器的位姿元组，如果计算失败则返回(None, None) |
| 备注 | 返回的KuavoPose包含位置(position)和方向(orientation)信息 |
| 函数名 | control_arm_joint_trajectory |
|---|---|
| 函数原型 | bool control_arm_joint_trajectory(list times, list joint_q) |
| 功能概述 | 控制机器人手臂关节轨迹。 |
| 参数 | times: 时间点列表，单位为秒 joint_q: 关节位置列表，每个时间点对应一组关节位置，单位为弧度 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 1. times和joint_q长度必须相同 2. 关节角度范围必须为[-π, π]弧度 3. 机器人必须处于站立状态 4. 手臂必须处于外部控制模式 |
| 函数名 | arm_ik |
|---|---|
| 函数原型 | list arm_ik(KuavoPose left_pose, KuavoPose right_pose, list left_elbow_pos_xyz=[0.0, |
0.0, 0.0], list right_elbow_pos_xyz=[0.0, 0.0, 0.0], list arm_q0=None, KuavoIKParams
params=None) |
| 功能概述 | 计算机器人手臂的逆向运动学。|
| 参数 | left_pose: 左手臂目标位姿
right_pose: 右手臂目标位姿
left_elbow_pos_xyz: 左肘位置<br/
right_elbow_pos_xyz: 右肘位置
arm_q0: 初始关节位置
params: 逆运动学参数 |
| 返回值 | 关节位置列表，如果计算失败则返回None |
| 备注 | 可以通过params参数调整计算精度和约束条件 |
| 函数名 | control_robot_end_effector_pose |
|---|---|
| 函数原型 | bool control_robot_end_effector_pose(KuavoPose left_pose, KuavoPose right_pose, KuavoManipulationMpcFrame frame) |
| 功能概述 | 控制机器人手臂末端执行器位姿。 |
| 参数 | left_pose: 左手臂目标位姿 right_pose: 右手臂目标位姿 frame: 控制坐标系 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 函数名 | set_fixed_arm_mode |
|---|---|
| 函数原型 | bool set_fixed_arm_mode() |
| 功能概述 | 设置手臂为固定模式。 |
| 返回值 | true: 设置成功 false: 设置失败 |
| 备注 | 固定模式下手臂保持当前位置不动 |
| 函数名 | set_auto_swing_arm_mode |
|---|---|
| 函数原型 | bool set_auto_swing_arm_mode() |
| 功能概述 | 设置手臂为自动摆动模式。 |
| 返回值 | true: 设置成功 false: 设置失败 |
| 备注 | 自动摆动模式下手臂会随步态自动摆动 |
| 函数名 | set_external_control_arm_mode |
|---|---|
| 函数原型 | bool set_external_control_arm_mode() |
| 功能概述 | 设置手臂为外部控制模式。 |
| 返回值 | true: 设置成功 false: 设置失败 |
| 备注 | 外部控制模式下可以通过control_arm_joint_positions等接口控制手臂 |
| 函数名 | arm_reset |
|---|---|
| 函数原型 | bool arm_reset() |
| 功能概述 | 重置手臂到默认位置。 |
| 返回值 | true: 重置成功 false: 重置失败 |
| 备注 | 通常用于初始化手臂位置 |
手部控制接口
KUAVO Humanoid SDK 支持两种类型的机械手：
- LejuClaw - 简单夹持器
- DexterousHand - 灵巧手
LejuClaw 接口
LejuClaw 类提供了以下主要接口：
| 函数名 | open |
|---|---|
| 函数原型 | bool open(EndEffectorSide side = EndEffectorSide.BOTH) |
| 功能概述 | 打开机械爪。 |
| 参数 | side: 控制哪一侧的机械爪，可选值： - LEFT: 左手 - RIGHT: 右手 - BOTH: 双手（默认） |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 调用后可使用 wait_for_finish() 等待动作完成 |
| 函数名 | close |
|---|---|
| 函数原型 | bool close(EndEffectorSide side = EndEffectorSide.BOTH) |
| 功能概述 | 关闭机械爪。 |
| 参数 | side: 控制哪一侧的机械爪，可选值同 open |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 调用后可使用 wait_for_finish() 等待动作完成 |
| 函数名 | control |
|---|---|
| 函数原型 | bool control(list target_positions, list target_velocities = None, list target_torques = None) |
| 功能概述 | 控制机械爪的位置、速度和力矩。 |
| 参数 | target_positions: 目标位置列表，长度为2（左右手各一个值），范围[0.0 ~ 100.0] target_velocities: 目标速度列表，长度为2，如果为None则使用默认值[90, 90] target_torques: 目标力矩列表，长度为2，如果为None则使用默认值[1.0, 1.0] |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 1. 调用后可使用 wait_for_finish() 等待动作完成 2. 如果机械爪仍在执行上一个动作，新的请求可能会被丢弃 |
| 函数名 | control_left |
|---|---|
| 函数原型 | bool control_left(list target_positions, list target_velocities = None, list target_torques = None) |
| 功能概述 | 控制左侧机械爪的位置、速度和力矩。 |
| 参数 | target_positions: 目标位置列表，长度为1，范围[0.0 ~ 100.0] target_velocities: 目标速度列表，长度为1，如果为None则使用默认值90 target_torques: 目标力矩列表，长度为1，如果为None则使用默认值1.0 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 1. 调用后可使用 wait_for_finish() 等待动作完成 2. 如果机械爪仍在执行上一个动作，新的请求可能会被丢弃 |
| 函数名 | control_right |
|---|---|
| 函数原型 | bool control_right(list target_positions, list target_velocities = None, list target_torques = None) |
| 功能概述 | 控制右侧机械爪的位置、速度和力矩。 |
| 参数 | target_positions: 目标位置列表，长度为1，范围[0.0 ~ 100.0] target_velocities: 目标速度列表，长度为1，如果为None则使用默认值90 target_torques: 目标力矩列表，长度为1，如果为None则使用默认值1.0 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 1. 调用后可使用 wait_for_finish() 等待动作完成 2. 如果机械爪仍在执行上一个动作，新的请求可能会被丢弃 |
DexterousHand 接口
DexterousHand 类提供了以下主要接口：
| 函数名 | control |
|---|---|
| 函数原型 | bool control(list target_positions, list target_velocities = None, list target_torques = None) |
| 功能概述 | 控制灵巧手的关节位置。 |
| 参数 | target_positions: 目标位置列表，长度为12（每只手6个关节），范围[0.0 ~ 100.0] target_velocities: 暂不支持 target_torques: 暂不支持 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 仅支持位置控制 |
| 函数名 | control_left |
| ------- | ------------- |
| 函数原型 | bool control_left(list target_positions, list target_velocities = None, list target_torques = None) |
| 功能概述 | 控制左手灵巧手的关节位置 |
| 参数 | target_positions: 目标位置列表，长度为6（左手6个关节），范围[0.0 ~ 100.0] target_velocities: 暂不支持 target_torques: 暂不支持 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 函数名 | control_right |
|---|---|
| 函数原型 | bool control_right(list target_positions, list target_velocities = None, list target_torques = None) |
| 功能概述 | 控制右手灵巧手的关节位置 |
| 参数 | target_positions: 目标位置列表，长度为6（右手6个关节），范围[0.0 ~ 100.0] target_velocities: 暂不支持 target_torques: 暂不支持 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 函数名 | make_gesture |
|---|---|
| 函数原型 | bool make_gesture(str l_gesture_name, str r_gesture_name) |
| 功能概述 | 控制灵巧手做预定义手势。 |
| 参数 | l_gesture_name: 左手手势名称 r_gesture_name: 右手手势名称 |
| 返回值 | true: 控制成功 false: 控制失败 |
| 备注 | 支持的手势包括：'fist', 'ok', 'thumbs_up', '666', 'number_1'等 |
示例
参考SDK示例代码：
头部控制：<kuavo-ros-opensource>/src/kuavo_humanoid_websocket_sdk/examples/atomic_skills/ctrl_head_example.py
演示如何通过SDK控制机器人头部的俯仰（pitch）和偏航（yaw）角度，实现头部的上下和左右运动。
示例代码中展示了两种运动模式：
头部上下点头运动：
- 从0度开始，先向下转到-25度
- 然后从-25度向上转到25度
- 最后从25度回到0度
- 重复2个周期
头部左右摇头运动：
- 从0度开始，先向左转到60度
- 然后从60度向右转到-60度
- 最后从-60度回到0度
- 重复2个周期
每次运动都以较小的角度增量(2度)变化，并设置了0.1秒的时间间隔，以实现平滑的运动效果。
手臂控制：<kuavo-ros-opensource>/src/kuavo_humanoid_websocket_sdk/examples/atomic_skills/ctrl_arm_example.py
显示手臂轨迹控制和目标姿势控制的示例。
包含关节角度插值运动、关节轨迹控制、末端执行器位姿控制等多种手臂操作方式，适合测试手臂的基本动作、轨迹跟踪和末端控制能力。
示例代码包含三个主要的控制函数：
control_arm_traj()：关节角度插值运动
- 从初始位置q0开始，通过90步插值运动到目标位置q1
- q1设置了右臂抬起50度的姿态
- 每步之间间隔0.02秒，实现平滑过渡
- 运动完成后恢复到初始位置
- 最后重置手臂位置
control_arm_joint_trajectory()：关节轨迹控制
- 定义了7个关键时间点的目标姿态
- 机器人会自动完成关键帧之间的轨迹规划
- 执行完毕后重置手臂位置
control_arm_end_effector_pose()：末端执行器位姿控制
- 直接指定左右手末端在世界坐标系中的目标位置和姿态
- 通过逆运动学自动计算所需的关节角度
- 机器人自动规划轨迹到达目标位姿
- 完成后重置手臂位置
机械爪控制：<kuavo-ros-opensource>/src/kuavo_humanoid_websocket_sdk/examples/atomic_skills/lejuclaw_example.py
演示如何控制LejuClaw夹持器末端执行器的示例，包括位置、速度和扭矩控制。
支持整体开合、单侧控制、目标位置设定等多种操作，通过 claw.open()、claw.close()、claw.control_left()、claw.control_right()、claw.control() 等接口实现。
示例代码展示了以下主要功能：
基本开合控制：
claw.close()完全闭合夹爪claw.open()完全打开夹爪- 每个动作后使用
wait_for_finish()等待动作完成
单侧夹爪控制：
claw.control_left([50])控制左侧夹爪到50度位置claw.control_right([80])控制右侧夹爪到80度位置- 可以分别精确控制左右两侧夹爪的位置
双侧同步控制：
claw.control([20, 100])同时控制左右夹爪到不同位置- 第一个参数控制左侧，第二个参数控制右侧
- 位置范围为0-100度，0表示完全闭合，100表示完全打开
注意事项：
- 每次动作后建议调用
wait_for_finish()等待完成，避免动作叠加 - 可以设置超时时间，如
wait_for_finish(timeout=2.0) - 连续发送指令时要注意等待前一个动作完成，否则可能被丢弃
灵巧手控制：<kuavo-ros-opensource>/src/kuavo_humanoid_websocket_sdk/examples/atomic_skills/dexhand_example.py
演示如何控制灵巧手（Dexterous Hand）的基本动作和预设手势。
支持单侧控制、双侧同步控制、预设手势调用等功能。示例代码展示了：
- 单侧手指控制：
- 左手指向"点赞"手势
- 右手指向"666"手势
- 预设手势调用：
- 获取所有支持的手势名称
- 调用预设手势（如"点赞"和"OK"手势）