kuavo-humanoid-sdk 1.1.2a924__py3-none-any.whl → 1.1.3a1239__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/dexterous_hand.py

@@ -4,7 +4,8 @@ from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.end_effector import EndEffector
 from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState, KuavoDexHandTouchState
 from kuavo_humanoid_sdk.kuavo.core.dex_hand_control import DexHandControl
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class DexterousHand(EndEffector):
     def __init__(self):
@@ -12,7 +13,10 @@ class DexterousHand(EndEffector):
                        'r_thumb', 'r_thumb_aux', 'r_index', 'r_middle', 'r_ring', 'r_pinky',]
         super().__init__(joint_names=joint_names)
         self.dex_hand_control = DexHandControl()
-        self._rb_state = KuavoRobotStateCore()
+        if GlobalConfig.use_websocket:
+            self._rb_state = KuavoRobotStateCoreWebsocket()
+        else:
+            self._rb_state = KuavoRobotStateCore()
 
     def control(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
         """Set the position of the hand.

kuavo_humanoid_sdk/kuavo/leju_claw.py

@@ -5,13 +5,17 @@ from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.end_effector import EndEffector
 from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState
 from kuavo_humanoid_sdk.kuavo.core.leju_claw_control import LejuClawControl
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class LejuClaw(EndEffector):
     def __init__(self):
         super().__init__(joint_names=['left_claw', 'right_claw'])
         self.leju_claw_control = LejuClawControl()
-        self._rb_state = KuavoRobotStateCore()
+        if GlobalConfig.use_websocket:
+            self._rb_state = KuavoRobotStateCoreWebsocket()
+        else:
+            self._rb_state = KuavoRobotStateCore()
 
     def control(self, target_positions: list, target_velocities:list=None, target_torques: list=None)->bool:
         """Control the claws to grip.

kuavo_humanoid_sdk/kuavo/robot.py

@@ -1,18 +1,17 @@
 #!/usr/bin/env python3
 # coding: utf-8
-from kuavo_humanoid_sdk.kuavo.core.ros_env import KuavoROSEnv
-kuavo_ros_env = KuavoROSEnv()
-if not kuavo_ros_env.Init():
-    raise RuntimeError("Failed to initialize ROS environment")
-else:
-    from typing import Tuple
-    from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
-    from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
-    from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
-    from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
-    from kuavo_humanoid_sdk.interfaces.robot import RobotBase
-    from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams
-    from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
+
+from kuavo_humanoid_sdk.kuavo.core.ros_env import KuavoROSEnv, KuavoROSEnvWebsocket
+from kuavo_humanoid_sdk.interfaces.robot import RobotBase
+from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+
+from typing import Tuple
+from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
+from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
+from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
 
 """
 Kuavo SDK - Python Interface for Kuavo Robot Control
@@ -45,19 +44,30 @@ class KuavoSDK:
        pass
 
     @staticmethod   
-    def Init(options:int=Options.Normal, log_level: str = "INFO")-> bool:
+    def Init(options:int=Options.Normal, log_level: str = "INFO", 
+             websocket_mode: bool = False, websocket_host='127.0.0.1', websocket_port=9090, websocket_timeout=5)-> bool:
         """Initialize the SDK.
-        
         Args:
             log_level (str): The logging level to use. Can be "ERROR", "WARN", "INFO", "DEBUG".
                 Defaults to "INFO".
-                
+            websocket_mode (bool): Whether to use websocket mode for ROS communication. If True,
+                the SDK will connect to ROS through websocket instead of direct ROS connection.
+                Defaults to False.
+            websocket_host (str): The host address of the rosbridge websocket server when using
+                websocket mode. Defaults to "127.0.0.1".
+            websocket_port (int): The port number of the rosbridge websocket server when using
+                websocket mode. Defaults to 9090.
+            websocket_timeout (int): The timeout for the websocket connection. Defaults to 5.
         Returns:
             bool: True if initialization is successful, False otherwise.
             
         Raises:
             RuntimeError: If the initialization fails.
         """
+        GlobalConfig.use_websocket = websocket_mode
+        GlobalConfig.websocket_host = websocket_host
+        GlobalConfig.websocket_port = websocket_port
+        GlobalConfig.websocket_timeout = websocket_timeout
         SDKLogger.setLevel(log_level.upper())
         # Initialize core components, connect ROS Topics...
         kuavo_core = KuavoRobotCore()
@@ -65,12 +75,18 @@ class KuavoSDK:
             debug = True
         else:
             debug = False   
-
         # Check if IK option is enabled
-        if options & KuavoSDK.Options.WithIK:
-            if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
-                print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
-                exit(1)
+        if GlobalConfig.use_websocket:
+            if options & KuavoSDK.Options.WithIK:
+                if not KuavoROSEnvWebsocket.check_rosnode_exists('/arms_ik_node'):
+                    print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
+                    exit(1)
+        else:
+            if options & KuavoSDK.Options.WithIK:
+                if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
+                    print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
+                    exit(1)
+
 
         if not kuavo_core.initialize(debug=debug):
             SDKLogger.error("[SDK] Failed to initialize core components.")
@@ -86,11 +102,16 @@ class KuavoSDK:
 class KuavoRobot(RobotBase):
     def __init__(self):
         super().__init__(robot_type="kuavo")
+        
+        if not GlobalConfig.use_websocket:
+            kuavo_ros_env = KuavoROSEnv()
+            if not kuavo_ros_env.Init():
+                raise RuntimeError("Failed to initialize ROS environment")
+
         self._robot_info = KuavoRobotInfo()
         self._robot_arm  = KuavoRobotArm()
         self._robot_head = KuavoRobotHead()
         self._kuavo_core = KuavoRobotCore()
-
     def stance(self)->bool:
         """Put the robot into 'stance' mode.
         

kuavo_humanoid_sdk/kuavo/robot_audio.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.kuavo.core.audio import KuavoRobotAudioCore   
+
+class KuavoRobotAudio:
+    """Audio system interface for controlling audio playback functionality of Kuavo humanoid robot.
+    
+    Provides functionality to play music files.
+    """
+    
+    def __init__(self):
+        """Initialize the audio system."""
+        self.audio = KuavoRobotAudioCore()
+
+    def play_audio(self, file_name: str, volume: float = 0.5, speed: float = 1.0) -> bool:
+        """Play the specified audio file.
+        
+        Args:
+            file_name (str): Name of the audio file to play
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        return self.audio.play_audio(file_name, volume, speed)
+    
+    def stop_music(self):
+        """Stop the currently playing audio."""
+        return self.audio.stop_music()
+    
+    def text_to_speech(self, text: str, volume: float = 0.5) -> bool:
+        """Synthesize and play the specified text.
+        
+        Args:
+            text (str): Text to be played
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        return self.audio.text_to_speech(text, volume)

kuavo_humanoid_sdk/kuavo/robot_info.py

@@ -2,7 +2,8 @@
 # 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
+from kuavo_humanoid_sdk.kuavo.core.ros.param import RosParameter, RosParamWebsocket, make_robot_param
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class KuavoRobotInfo(RobotInfoBase):
     def __init__(self, robot_type: str = "kuavo"):
@@ -10,7 +11,11 @@ class KuavoRobotInfo(RobotInfoBase):
         
         # Load robot parameters from ROS parameter server
         kuavo_ros_param = make_robot_param()
-        self._ros_param = RosParameter()
+        if GlobalConfig.use_websocket:
+            self._ros_param = RosParamWebsocket()
+        else:
+            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']

kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -4,11 +4,15 @@ import time
 from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.data_types import (
     KuavoImuData, KuavoJointData, KuavoOdometry, KuavoArmCtrlMode,EndEffectorState)
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class KuavoRobotState:
     def __init__(self, robot_type: str = "kuavo"):
-        self._rs_core = KuavoRobotStateCore()
+        if GlobalConfig.use_websocket:
+            self._rs_core = KuavoRobotStateCoreWebsocket()
+        else:
+            self._rs_core = KuavoRobotStateCore()
 
     @property
     def imu_data(self) -> KuavoImuData:

kuavo_humanoid_sdk/kuavo/robot_tool.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix)
+from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCore
+from typing import Union
+
+class KuavoRobotTools:
+    """Robot tools class providing coordinate frame transformation interfaces.
+    
+    This class encapsulates coordinate transformation queries between different robot frames,
+    supporting multiple return data formats.
+    """
+    
+    def __init__(self):
+        """Initialize robot tools instance."""
+        self.tools_core = KuavoRobotToolsCore()
+
+    def get_tf_transform(self, target_frame: str, source_frame: str, 
+                       return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Get transformation between specified coordinate frames.
+        
+        Args:
+            target_frame (str): Name of target coordinate frame
+            source_frame (str): Name of source coordinate frame
+            return_type (str, optional): Return data format type. Valid values: 
+                "pose_quaternion" - quaternion pose format,
+                "homogeneous" - homogeneous matrix format. Defaults to "pose_quaternion".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 
+                Transformation data in specified format, or None if failed
+        
+        Raises:
+            ValueError: If invalid return_type is provided
+        """
+        return self.tools_core._get_tf_tree_transform(target_frame, source_frame, return_type=return_type)
+
+    def get_base_to_odom(self, return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Get transformation from base_link to odom frame.
+        
+        Args:
+            return_type (str, optional): Return format type. Same as get_tf_transform. 
+                Defaults to "pose_quaternion".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Transformation data or None
+        """
+        return self.get_tf_transform("odom", "base_link", return_type)
+
+    def get_camera_to_base(self, return_type: str = "homogeneous") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Get transformation from camera_link to base_link frame.
+        
+        Args:
+            return_type (str, optional): Return format type. Same as get_tf_transform.
+                Defaults to "homogeneous".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Transformation data or None
+        """
+        return self.get_tf_transform("base_link", "camera_link", return_type)

kuavo_humanoid_sdk/kuavo/robot_vision.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData)
+from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore
+
+try:
+    from apriltag_ros.msg import AprilTagDetectionArray
+except ImportError:
+    pass
+
+class KuavoRobotVision:
+    """Vision system interface for Kuavo humanoid robot.
+    
+    Provides access to AprilTag detection data from different coordinate frames.
+    
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """Initialize the vision system.
+        
+        Args:
+            robot_type (str, optional): Robot type identifier. Defaults to "kuavo".
+        """
+        if not hasattr(self, '_initialized'):
+            self._vision_core = KuavoRobotVisionCore()
+
+    def get_data_by_id(self, target_id: int, data_source: str = "base") -> dict:
+        """Get AprilTag detection data for a specific ID from specified source.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            data_source (str, optional): Data source frame. Can be "base", "camera", 
+                or "odom". Defaults to "base".
+                
+        Returns:
+            dict: Detection data containing position, orientation and metadata
+        """
+        return self._vision_core._get_data_by_id(target_id, data_source)
+    
+    def get_data_by_id_from_camera(self, target_id: int) -> dict:
+        """Get AprilTag data from camera coordinate frame.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            
+        Returns:
+            dict: See get_data_by_id() for return format
+        """
+        return self._vision_core._get_data_by_id(target_id, "camera")
+    
+    def get_data_by_id_from_base(self, target_id: int) -> dict:
+        """Get AprilTag data from base coordinate frame.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            
+        Returns:
+            dict: See get_data_by_id() for return format
+        """
+        return self._vision_core._get_data_by_id(target_id, "base")
+    
+    def get_data_by_id_from_odom(self, target_id: int) -> dict:
+        """Get AprilTag data from odom coordinate frame.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            
+        Returns:
+            dict: See get_data_by_id() for return format
+        """
+        return self._vision_core._get_data_by_id(target_id, "odom")
+    
+    @property
+    def apriltag_data_from_camera(self) -> AprilTagData:
+        """AprilTagData: All detected AprilTags in camera frame (property)"""
+        return self._vision_core.apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self) -> AprilTagData:
+        """AprilTagData: All detected AprilTags in camera frame (property)"""
+        return self._vision_core.apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self) -> AprilTagData:
+        """AprilTagData: All detected AprilTags in camera frame (property)"""
+        return self._vision_core.apriltag_data_from_odom
+    
+