kuavo-humanoid-sdk 1.2.1b3279__20250911210455-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/robot_head.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+import asyncio
+import threading
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+import json
+
+class KuavoRobotHead:
+    """机器人头部控制类"""
+    def __init__(self):
+        self._kuavo_core = KuavoRobotCore()
+
+    def _send_log(self, message: str):
+        """发送日志到8889端口的辅助方法"""
+        self._kuavo_core.logger.send_log(message)
+            
+        
+    def control_head(self, yaw: float, pitch: float) -> bool:
+        """控制机器人的头部关节运动。
+        
+        Args:
+            yaw (float): 头部的偏航角,单位弧度,范围[-1.396, 1.396](-80到80度)。
+            pitch (float): 头部的俯仰角,单位弧度,范围[-0.436, 0.436](-25到25度)。
+            
+        Returns:
+            bool: 如果头部控制成功返回True,否则返回False。
+        """
+        # 发送开始控制头部的日志
+        self._send_log(f"开始控制头部运动: yaw={yaw:.3f}, pitch={pitch:.3f}")
+        
+        limited_yaw = yaw
+        limited_pitch = pitch
+        
+        # 原有的代码逻辑保持不变
+        # Check yaw limits (-80 to 80 degrees)
+        if yaw < -math.pi*4/9 or yaw > math.pi*4/9:  # -80 to 80 degrees in radians
+            SDKLogger.warn(f"[Robot] yaw {yaw} exceeds limit [-{math.pi*4/9:.3f}, {math.pi*4/9:.3f}] radians (-80 to 80 degrees), will be limited")
+            limited_yaw = min(math.pi*4/9, max(-math.pi*4/9, yaw))
+            self._send_log(f"yaw值超限，已限制为: {limited_yaw:.3f}")
+            
+        # Check pitch limits (-25 to 25 degrees)
+        if pitch < -math.pi/7.2 - 0.001 or pitch > math.pi/7.2 + 0.001:  # -25 to 25 degrees in radians
+            SDKLogger.warn(f"[Robot] pitch {pitch} exceeds limit [-{math.pi/7.2:.3f}, {math.pi/7.2:.3f}] radians (-25 to 25 degrees), will be limited")
+            limited_pitch = min(math.pi/7.2, max(-math.pi/7.2, pitch))
+            self._send_log(f"pitch值超限，已限制为: {limited_pitch:.3f}")
+        
+        # 执行头部控制
+        result = self._kuavo_core.control_robot_head(yaw=limited_yaw, pitch=limited_pitch)
+        
+        # 发送执行结果日志
+        self._send_log(f"头部控制完成: yaw={limited_yaw:.3f}, pitch={limited_pitch:.3f}, 结果={'成功' if result else '失败'}")
+        
+        return result
+    def enable_head_tracking(self, target_id: int)->bool:
+        """启用头部跟踪功能，在机器人运动过程中，头部将始终追踪指定的 Apriltag ID
+
+        Args:
+            target_id (int): 目标ID。
+
+        Returns:
+            bool: 如果启用成功返回True，否则返回False。
+        """
+        return self._kuavo_core.enable_head_tracking(target_id)
+    
+    def disable_head_tracking(self)->bool:
+        """禁用头部跟踪功能。
+
+        Returns:
+            bool: 如果禁用成功返回True，否则返回False。
+        """
+        return self._kuavo_core.disable_head_tracking()
+

kuavo_humanoid_sdk/kuavo/robot_info.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from typing import Tuple
+from kuavo_humanoid_sdk.interfaces.robot_info import RobotInfoBase
+from kuavo_humanoid_sdk.kuavo.core.ros.param import RosParameter, make_robot_param
+
+class KuavoRobotInfo(RobotInfoBase):
+    def __init__(self, robot_type: str = "kuavo"):
+        super().__init__(robot_type=robot_type)
+        
+        # Load robot parameters from ROS parameter server
+        kuavo_ros_param = make_robot_param()
+        self._ros_param = RosParameter()
+            
+        self._robot_version = kuavo_ros_param['robot_version']
+        self._end_effector_type = kuavo_ros_param['end_effector_type']
+        self._arm_joint_dof = kuavo_ros_param['arm_dof']
+        self._joint_dof = kuavo_ros_param['arm_dof'] + kuavo_ros_param['leg_dof'] + kuavo_ros_param['head_dof']
+        self._joint_names = kuavo_ros_param['joint_names']
+        self._end_frames_names = kuavo_ros_param['end_frames_names']
+        self._head_joint_dof = kuavo_ros_param['head_dof']
+        self._head_joint_names = self._joint_names[-2:]
+        self._arm_joint_names = self._joint_names[12:self._arm_joint_dof + 12]
+        self._init_stand_height = kuavo_ros_param['init_stand_height']
+
+    @property
+    def robot_version(self) -> str:
+        """返回 Kuavo 机器人的版本。
+
+        Returns:
+            str: 机器人版本号，例如 "42"、"45" 等。
+        """
+        return self._robot_version
+
+    @property
+    def end_effector_type(self) -> str:
+        """返回 Kuavo 机器人末端执行器的类型。
+
+        Returns:
+            str: 末端执行器类型，其中：
+                - ``qiangnao`` 表示普通灵巧手
+                - ``lejuclaw`` 表示乐聚二指夹爪
+                - ``qiangnao_touch`` 表示触觉灵巧手
+                - ...
+        """
+        return self._end_effector_type
+
+    @property
+    def joint_names(self) -> list:
+        """返回 Kuavo 机器人所有关节的名称。
+
+        Returns:
+            list: 包含所有关节名称的列表。
+        """
+        return self._joint_names
+
+    @property
+    def joint_dof(self) -> int:
+        """返回 Kuavo 机器人的总关节数。
+
+        Returns:
+            int: 总关节数，例如 28。
+        """
+        return self._joint_dof
+
+    @property
+    def arm_joint_dof(self) -> int:
+        """返回 Kuavo 机器人双臂的关节数。
+
+        Returns:
+            int: 双臂的关节数，例如 14。 
+        """
+        return self._arm_joint_dof
+
+    @property
+    def arm_joint_names(self) -> list:
+        """返回 Kuavo 机器人双臂关节的名称。
+
+        Returns:
+            list: 包含双臂关节名称的列表。
+        """
+        return self._arm_joint_names
+
+    @property
+    def head_joint_dof(self) -> int:
+        """返回 Kuavo 机器人头部的关节数。
+
+        Returns:
+            int: 头部的关节数，例如 2。
+        """
+        return self._head_joint_dof
+
+    @property
+    def head_joint_names(self) -> list:
+        """返回 Kuavo 机器人头部关节的名称。
+
+        Returns:
+            list: 包含头部关节名称的列表。
+        """
+        return self._head_joint_names
+
+    @property
+    def eef_frame_names(self) -> Tuple[str, str]:
+        """返回 Kuavo 机器人末端执行器坐标系的名称。
+
+        Returns:
+            Tuple[str, str]: 包含末端执行器坐标系名称的元组，其中：\n
+                - 第一个元素是左手坐标系名称\n
+                - 第二个元素是右手坐标系名称\n
+                例如 ("zarm_l7_link", "zarm_r7_link") \n
+        """
+        return self._end_frames_names[1], self._end_frames_names[2]
+
+    @property
+    def init_stand_height(self) -> float:
+        """返回 Kuavo 机器人初始化站立时的质心高度。
+
+        Returns:
+            float: 初始化站立时的质心高度
+        """
+        return self._init_stand_height
+    
+    def __str__(self) -> str:
+        return (
+            f"KuavoRobotInfo("
+            f"robot_type={self.robot_type}, "
+            f"robot_version={self.robot_version}, "
+            f"end_effector_type={self.end_effector_type}, "
+            f"joint_names={self.joint_names}, "
+            f"joint_dof={self.joint_dof}, "
+            f"arm_joint_dof={self.arm_joint_dof}, "
+            f"init_stand_height={self.init_stand_height}"
+            f")"
+        )

kuavo_humanoid_sdk/kuavo/robot_microphone.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.kuavo.core.microphone import RobotMicrophoneCore
+
+class RobotMicrophone: 
+    def __init__(self):
+        """Initialize the microphone system."""
+        self.microphone = RobotMicrophoneCore()
+
+    def wait_for_wake_word(self, timeout_sec: int = 60) -> bool:
+        """Wait for the wake word to be detected.
+        
+        Args:
+            timeout_sec (int): Timeout in seconds
+                
+        Returns:
+            bool: True if the wake word was detected within the timeout, False otherwise
+        """
+        return self.microphone.wait_for_wake_word(timeout_sec=timeout_sec)

kuavo_humanoid_sdk/kuavo/robot_navigation.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.kuavo.core.navigation import KuavoRobotNavigationCore, NavigationStatus
+import tf
+from geometry_msgs.msg import Pose, Point, Quaternion
+import rospy
+import time
+
+class RobotNavigation:
+    """机器人导航接口类。"""
+
+    def __init__(self):
+        """初始化 RobotNavigation 对象。"""
+        self.robot_navigation = KuavoRobotNavigationCore()
+
+    def navigate_to_goal(
+        self, x: float, y: float, z: float, roll: float, pitch: float, yaw: float
+    ) -> bool:
+        """导航到指定目标位置。
+
+        Args:
+            x (float): 目标点的x坐标。
+            y (float): 目标点的y坐标。
+            z (float): 目标点的z坐标。
+            roll (float): 目标点的横滚角。
+            pitch (float): 目标点的俯仰角。
+            yaw (float): 目标点的偏航角。
+
+        Returns:
+            bool: 导航是否成功。
+        """
+        orientation = tf.transformations.quaternion_from_euler(yaw, pitch, roll)
+        goal = Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3]))
+        self.robot_navigation.navigate_to_goal(goal)
+        while self.get_current_status() is not NavigationStatus.ACTIVE:
+            time.sleep(0.01)
+        while not rospy.is_shutdown():
+            if self.get_current_status() == NavigationStatus.SUCCEEDED:
+                break
+            time.sleep(0.01)
+        return True
+
+    def navigate_to_task_point(self, task_point_name: str) -> bool:
+        """导航到指定的任务点。
+
+        Args:
+            task_point_name (str): 任务点的名称。
+
+        Returns:
+            bool: 导航是否成功。
+        """
+        self.robot_navigation.navigate_to_task_point(task_point_name)
+        while self.get_current_status() is not NavigationStatus.ACTIVE:
+            time.sleep(0.01)
+        while not rospy.is_shutdown():
+            if self.get_current_status() == NavigationStatus.SUCCEEDED:
+                break
+            time.sleep(0.01)
+        return True
+
+    def stop_navigation(self) -> bool:
+        """停止导航。
+
+        Returns:
+            bool: 停止导航是否成功。
+        """
+        return self.robot_navigation.stop_navigation()
+
+    def get_current_status(self) -> str:
+        """获取当前导航状态。
+
+        Returns:
+            str: 当前导航状态。
+        """
+        return self.robot_navigation.get_current_status()
+
+    def init_localization_by_pose(
+        self, x: float, y: float, z: float, roll: float, pitch: float, yaw: float
+    ) -> bool:
+        """通过位姿初始化定位。
+
+        Args:
+            x (float): 位姿的x坐标。
+            y (float): 位姿的y坐标。
+            z (float): 位姿的z坐标。
+            roll (float): 位姿的横滚角。
+            pitch (float): 位姿的俯仰角。
+            yaw (float): 位姿的偏航角。
+
+        Returns:
+            bool: 定位初始化是否成功。
+        """
+        orientation = tf.transformations.quaternion_from_euler(yaw, pitch, roll)
+        pose = Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3]))
+        return self.robot_navigation.init_localization_by_pose(pose)
+    
+    def init_localization_by_task_point(
+        self, task_point_name: str
+    ) -> bool:
+        """通过任务点初始化定位。
+
+        Args:
+            task_point_name (str): 任务点的名称。
+
+        Returns:
+            bool: 定位初始化是否成功。
+        """
+        return self.robot_navigation.init_localization_by_task_point(task_point_name)
+
+    def load_map(self, map_name: str) -> bool:
+        """加载地图。
+
+        Args:
+            map_name (str): 地图名称。
+
+        Returns:
+            bool: 加载地图是否成功。
+        """
+        return self.robot_navigation.load_map(map_name)
+
+    def get_all_maps(self) -> list:
+        """获取所有地图名称。
+
+        Returns:
+            list: 地图名称列表。
+        """
+        return self.robot_navigation.get_all_maps()
+
+    def get_current_map(self) -> str:
+        """获取当前地图名称。
+
+        Returns:
+            str: 当前地图名称。
+        """
+        return self.robot_navigation.get_current_map()

kuavo_humanoid_sdk/kuavo/robot_observation.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.interfaces.data_types import (
+    KuavoJointCommand, KuavoTwist)
+from kuavo_humanoid_sdk.kuavo.core.ros.observation import KuavoRobotObservationCore
+
+class KuavoRobotObservation:
+    """用于访问机器人观测数据的类。
+    
+    该类提供了一个高级接口来访问机器人的观测数据，包括关节命令、速度命令和姿态命令。
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """初始化机器人观测接口
+        
+        Args:
+            robot_type (str): 机器人类型。默认为"kuavo"
+        """
+        self._obs_core = KuavoRobotObservationCore()
+
+    @property
+    def joint_command(self) -> KuavoJointCommand:
+        """获取当前关节控制命令
+        
+        Returns:
+            KuavoJointCommand: 包含所有机器人关节的位置、速度和力矩命令的对象。
+        """
+        return self._obs_core.joint_command
+
+    @property
+    def cmd_vel(self) -> KuavoTwist:
+        """获取当前 cmd_vel 速度控制命令
+        
+        Returns:
+            KuavoTwist: 包含线速度(m/s)和角速度(rad/s)命令的对象。
+        """
+        return self._obs_core.cmd_vel
+
+    @property
+    def cmd_pose(self) -> KuavoTwist:
+        """获取当前 cmd_pose 姿态控制命令
+        
+        Returns:
+            KuavoTwist: 包含线性姿态命令(m)和角度姿态命令(rad)的对象。
+        """
+        return self._obs_core.cmd_pose
+
+    @property    
+    def arm_position_command(self) -> list:
+        """获取手臂关节的位置控制命令
+        
+        Returns:
+            list: 手臂关节(索引12-25)的位置命令，单位为弧度。
+        """
+        return self._obs_core.arm_position_command
+
+    @property
+    def head_position_command(self) -> list:
+        """获取头部关节的位置控制命令
+        
+        Returns:
+            list: 头部关节(索引26-27)的位置命令，单位为弧度。
+        """
+        return self._obs_core.head_position_command

kuavo_humanoid_sdk/kuavo/robot_speech.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.kuavo.core.llm_doubao import RobotLLMDoubaoCore
+
+class RobotSpeech:
+    """
+    RobotSpeech is a wrapper class that provides an interface to interact with the Doubao speech system.
+    """
+
+    def __init__(self):
+        """Initialize ``RobotSpeech`` wrapper and the underlying speech core."""
+        self.doubao_speech_core = RobotLLMDoubaoCore()
+
+    def establish_doubao_speech_connection(self, app_id: str, access_key: str) -> bool:
+        """Establish a connection to the Doubao service."""
+        return self.doubao_speech_core.verify_connection(app_id, access_key)
+
+    def start_speech(self):
+        """Start the speech system."""
+        self.doubao_speech_core.start_speech_system()
+
+    def stop_speech(self):
+        """Stop the speech system."""
+        self.doubao_speech_core.stop_speech_system()