头部及手部控制

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_position
函数原型	bool control_arm_position(list joint_positions)
功能概述	控制机器人手臂关节位置。
参数	joint_positions: 目标关节位置列表，单位为弧度，长度为手臂关节数量（通常为14个关节）
返回值	true: 控制成功 false: 控制失败
备注	关节角度范围通常为[-π, π]弧度

函数名	arm_fk
函数原型	Tuple[KuavoPose, KuavoPose] arm_fk(list q)
功能概述	计算机器人手臂的正向运动学。
参数	q: 关节位置列表，单位为弧度
返回值	左右手臂末端执行器的位姿元组，如果计算失败则返回(None, None)
备注	返回的KuavoPose包含位置(position)和方向(orientation)信息

函数名	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: 左肘位置 right_elbow_pos_xyz: 右肘位置 arm_q0: 初始关节位置 params: 逆运动学参数
返回值	关节位置列表，如果计算失败则返回None
备注	可以通过params参数调整计算精度和约束条件

函数名	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_position等接口控制手臂

函数名	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: 控制失败
备注	仅支持位置控制

函数名	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'等

示例

头部控制示例

from kuavo_humanoid_sdk import KuavoSDK, KuavoRobot
import math
import time

def main():
    # 初始化SDK
    if not KuavoSDK().Init():
        print("SDK初始化失败！")
        exit(1)
    
    robot = KuavoRobot()
    
    # 控制头部左右转动
    robot.control_head(yaw=math.radians(30), pitch=0.0)  # 向右转30度
    time.sleep(2)
    robot.control_head(yaw=math.radians(-30), pitch=0.0)  # 向左转30度
    time.sleep(2)
    
    # 控制头部上下点头
    robot.control_head(yaw=0.0, pitch=math.radians(20))  # 向上抬20度
    time.sleep(2)
    robot.control_head(yaw=0.0, pitch=math.radians(-20))  # 向下低20度
if __name__ == "__main__":
    main()

机械爪控制示例

from kuavo_humanoid_sdk import KuavoSDK, LejuClaw
from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide


def main():
    # 初始化SDK
    if not KuavoSDK().Init():
        print("SDK初始化失败！")
        exit(1)
    claw = LejuClaw()
    
    # 控制左手机械爪
    claw.open(EndEffectorSide.LEFT)  # 打开左手
    claw.wait_for_finish()  # 等待动作完成
    time.sleep(1)
    
    claw.close(EndEffectorSide.LEFT)  # 关闭左手
    robot.end_effector.wait_for_finish()  # 等待动作完成
if __name__ == "__main__":
    main()

灵巧手控制示例

from kuavo_humanoid_sdk import KuavoSDK, DexterousHand
import time

def main():
    # 初始化SDK
    if not KuavoSDK().Init():
        print("SDK初始化失败！")
        exit(1)
    
    robot = KuavoRobot()
    
    # 使用预定义手势
    dex_hand = DexterousHand()
    dex_hand.make_gesture("ok", "thumbs_up")  # 左手OK手势，右手竖大拇指
    time.sleep(2)
    
    # 自定义手指位置
    positions = [50.0] * 12  # 所有手指设置到中间位置
    dex_hand.control(target_positions=positions)
if __name__ == "__main__":
    main()

手臂控制示例

from kuavo_humanoid_sdk import KuavoSDK, KuavoRobot,  KuavoRobotState
import math
import time

def main():
    # 初始化SDK
    if not KuavoSDK().Init():
        print("SDK初始化失败！")
        exit(1)
    
    robot = KuavoRobot()
    robot_state = KuavoRobotState()
    
    # 进入站立状态
    robot.stance()
    robot_state.wait_for_stance()
    
    # 设置手臂为外部控制模式
    robot.set_external_control_arm_mode()
    
    # 获取当前手臂关节状态
    current_joints = robot_state.arm_joint_state().position
    
    # 修改左臂肘部关节角度（举起手臂）
    new_joints = current_joints.copy()
    new_joints[2] = math.radians(45)  # 左肘关节
    
    # 控制手臂到新位置
    robot.control_arm_position(new_joints)
    time.sleep(2)
    
    # 重置手臂位置
    robot.arm_reset()
if __name__ == "__main__":
    main()

手臂逆运动学示例

from kuavo_humanoid_sdk import KuavoSDK, KuavoRobot, KuavoPose
from kuavo_humanoid_sdk import KuavoRobotState
import time
import math

def main():
    # 初始化SDK
    if not KuavoSDK().Init():
        print("SDK初始化失败！")
        exit(1)
    
    robot = KuavoRobot()
    robot.arm_reset()
    robot_state = KuavoRobotState()
    
    # 进入站立状态
    robot.stance()
    robot_state.wait_for_stance()
    
    # 设置手臂为外部控制模式
    robot.set_external_control_arm_mode()
    
    # 获取当前手臂末端位姿
    current_joints = robot_state.arm_joint_state().position
    left_pose, right_pose = robot.arm_fk(current_joints)
    
    # 修改左手位置（向前伸出）
    new_left_pose = KuavoPose(
        position=(left_pose.position[0] + 0.1, left_pose.position[1], left_pose.position[2]),
        orientation=left_pose.orientation
    )
    
    # 计算逆运动学
    new_joints = robot.arm_ik(new_left_pose, right_pose)
    
    if new_joints:
        # 控制手臂到新位置
        robot.control_arm_position(new_joints)
    else:
        print("逆运动学计算失败")
if __name__ == "__main__":
    main()

详细示例请参考SDK示例代码：

头部控制：<kuavo-ros-opensource>/src/kuavo_humanoid_sdk/examples/ctrl_head_example.py
手臂控制：<kuavo-ros-opensource>/src/kuavo_humanoid_sdk/examples/ctrl_arm_example.py
机械爪控制：<kuavo-ros-opensource>/src/kuavo_humanoid_sdk/examples/lejuclaw_example.py
灵巧手控制：<kuavo-ros-opensource>/src/kuavo_humanoid_sdk/examples/dexhand_example.py
逆运动学：<kuavo-ros-opensource>/src/kuavo_humanoid_sdk/examples/arm_ik_example.py

头部及手部控制

接口说明​

头部控制接口​

KuavoRobotHead​

手臂控制接口​

KuavoRobotArm​

手部控制接口​

LejuClaw 接口​

DexterousHand 接口​

示例​

头部控制示例​

机械爪控制示例​

灵巧手控制示例​

手臂控制示例​

手臂逆运动学示例​