kuavo-humanoid-sdk 1.2.2__20250922181216-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/dex_hand_control.py

@@ -0,0 +1,114 @@
+import copy
+import threading
+from queue import Queue
+from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
+class DexHandControl:
+    _instance = None
+
+    def __new__(cls, *args, **kwargs):
+        if not cls._instance:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+
+    def __init__(self):
+        if not hasattr(self, '_initialized'):
+            self.queue = Queue()  # Initialize a queue to hold commands
+            self._kuavo_core = KuavoRobotCore()
+            self.thread = threading.Thread(target=self._process_queue)  # Create a thread to process the queue
+            self.thread.daemon = True  # Set the thread as a daemon so it will exit when the main program exits
+            self.thread.start()  # Start the thread
+            kuavo_info = make_robot_param()
+            self._ee_type = kuavo_info['end_effector_type']
+            # Initialize last command position, torque, and velocity
+            self.last_cmd_position = {EndEffectorSide.LEFT: [0] * 6, EndEffectorSide.RIGHT: [0] * 6}
+            self._initialized = True
+
+    def control(self, target_positions: list, side: EndEffectorSide):
+        self.queue.put(('position', EndEffectorSide(side.value), target_positions))
+
+    def control_velocity(self, target_velocities: list, side: EndEffectorSide):
+        self.queue.put(('velocity', EndEffectorSide(side.value), target_velocities))
+
+    def make_gestures(self, gestures:list)->bool:
+        """
+            Make a gesture for the dexhand.
+            Args:
+                gestures: list of gestures to make.
+                    [{'gesture_name': 'name', 'side': EndEffectorSide.LEFT},...]
+        """
+        exec_gs = []
+        for gs in gestures:
+            side = gs['hand_side']
+            gesture_name = gs['gesture_name']
+            if side == EndEffectorSide.LEFT:
+                exec_gs.append({'gesture_name': gesture_name, 'hand_side': 0})
+            elif side == EndEffectorSide.RIGHT:
+                exec_gs.append({'gesture_name': gesture_name, 'hand_side': 1})
+            elif side == EndEffectorSide.BOTH:
+                exec_gs.append({'gesture_name': gesture_name, 'hand_side': 2})
+
+        if len(exec_gs) == 0:
+            SDKLogger.error('No gestures to make')
+            return False
+        
+        # Make gestures
+        self._kuavo_core.execute_gesture(exec_gs)
+
+    def get_gesture_names(self)->list:
+        """
+            Get the names of all gestures.
+        """
+        gs = self._kuavo_core.get_gesture_names()
+        if not gs:
+            return None
+        return gs
+    
+    
+    def _process_dexhand_command(self, command, side, data):
+        if command == 'position':
+            pos = self.last_cmd_position[EndEffectorSide.LEFT] + self.last_cmd_position[EndEffectorSide.RIGHT]
+            if side == EndEffectorSide.BOTH:
+                pos = copy.deepcopy(data)
+            elif side == EndEffectorSide.LEFT:
+                pos[:6] = data
+            elif side == EndEffectorSide.RIGHT:
+                pos[6:] = data
+            else:
+                return
+            self._kuavo_core.control_robot_dexhand(left_position=pos[:6], right_position=pos[6:])                       
+            self.last_cmd_position[EndEffectorSide.LEFT] = pos[:6]
+            self.last_cmd_position[EndEffectorSide.RIGHT] = pos[6:]
+
+    def _process_touch_dexhand_command(self, command, side, data):
+        if command == 'position':
+            ctrl_mode = 0
+            if side == EndEffectorSide.BOTH:
+                self._kuavo_core.robot_dexhand_command(data, ctrl_mode, 2)
+            elif side == EndEffectorSide.LEFT:
+                self._kuavo_core.robot_dexhand_command(data, ctrl_mode, 0)
+            elif side == EndEffectorSide.RIGHT:
+                self._kuavo_core.robot_dexhand_command(data, ctrl_mode, 1)
+        elif command == 'velocity':
+            ctrl_mode = 0
+            if side == EndEffectorSide.BOTH:
+                self._kuavo_core.robot_dexhand_command(data, ctrl_mode, 2)
+            elif side == EndEffectorSide.LEFT:
+                self._kuavo_core.robot_dexhand_command(data, ctrl_mode, 0)
+            elif side == EndEffectorSide.RIGHT:
+                self._kuavo_core.robot_dexhand_command(data, ctrl_mode, 1)
+
+    def _process_queue(self):
+        while True:
+            try:
+                command, side, data = self.queue.get()  # This will block until an item is available in the queue
+                SDKLogger.debug(f'[DexHandControl] Received command: {command}, for side: {side}, with data: {data}')
+                if self._ee_type == EndEffectorType.QIANGNAO:
+                    self._process_dexhand_command(command, side, data)
+                elif self._ee_type == EndEffectorType.QIANGNAO_TOUCH:
+                    self._process_touch_dexhand_command(command, side, data)    
+                self.queue.task_done()
+            except KeyboardInterrupt:
+                break

kuavo_humanoid_sdk/kuavo/core/leju_claw_control.py

@@ -0,0 +1,67 @@
+import threading
+from queue import Queue
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+class LejuClawControl:
+    _instance = None
+
+    def __new__(cls, *args, **kwargs):
+        if not cls._instance:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+    
+    def __init__(self):
+        if not hasattr(self, '_initialized'):
+            self._initialized = True
+            self.queue = Queue()  # Initialize a queue to hold commands
+            self._kuavo_core = KuavoRobotCore()
+            self.thread = threading.Thread(target=self._process_queue)  # Create a thread to process the queue
+            self.thread.daemon = True  # Set the thread as a daemon so it will exit when the main program exits
+            self.thread.start()  # Start the thread
+            self.last_cmd_position = {EndEffectorSide.LEFT: 0, EndEffectorSide.RIGHT: 0}
+            self.last_cmd_torque = {EndEffectorSide.LEFT: 1.0, EndEffectorSide.RIGHT: 1.0}
+            self.last_cmd_velocity = {EndEffectorSide.LEFT: 90, EndEffectorSide.RIGHT: 90}
+    def control(self, position:list, velocity:list, torque:list, side:EndEffectorSide):
+        self.queue.put((EndEffectorSide(side.value), position, velocity, torque))
+
+    def release(self, side:EndEffectorSide):
+        if side == EndEffectorSide.BOTH:
+            self.queue.put((EndEffectorSide.BOTH, [0.0, 0.0], [90, 90], [1.0, 1.0]))
+        else:    
+            self.queue.put((EndEffectorSide(side.value), [0], [90], [1.0]))
+
+    def _process_queue(self):
+        while True:
+            try:
+                side, q, v, tau = self.queue.get()
+                SDKLogger.debug(f"[LejuClawControl] Received command:  {side} to {q} with {v} and {tau}")
+                postions = [self.last_cmd_position[EndEffectorSide.LEFT], self.last_cmd_position[EndEffectorSide.RIGHT]]
+                velocities = [self.last_cmd_velocity[EndEffectorSide.LEFT], self.last_cmd_velocity[EndEffectorSide.RIGHT]]
+                torques = [self.last_cmd_torque[EndEffectorSide.LEFT], self.last_cmd_torque[EndEffectorSide.RIGHT]]
+                if side == EndEffectorSide.LEFT:
+                    postions[0] = q[0]
+                    velocities[0] = v[0]
+                    torques[0] = tau[0]
+                elif side == EndEffectorSide.RIGHT:    
+                    postions[1] = q[0]
+                    velocities[1] = v[0]
+                    torques[1] = tau[0]
+                else: # both
+                    postions = q
+                    velocities = v
+                    torques = tau
+
+                # call ros service.
+                self._kuavo_core.control_leju_claw(postions, velocities, torques)
+                # update last cmd
+                self.last_cmd_position[EndEffectorSide.LEFT] = postions[0]
+                self.last_cmd_position[EndEffectorSide.RIGHT] = postions[1]
+                self.last_cmd_velocity[EndEffectorSide.LEFT] = velocities[0]
+                self.last_cmd_velocity[EndEffectorSide.RIGHT] = velocities[1]
+                self.last_cmd_torque[EndEffectorSide.LEFT] = torques[0]
+                self.last_cmd_torque[EndEffectorSide.RIGHT] = torques[1]
+                # mark task as done
+                self.queue.task_done()
+            except KeyboardInterrupt:
+                break

kuavo_humanoid_sdk/kuavo/core/ros/audio.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+
+try:    
+    import rospy
+    from std_msgs.msg import Bool
+    from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import  playmusic, playmusicRequest
+    from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import  SpeechSynthesis, SpeechSynthesisRequest
+except:
+    pass
+
+class Audio:
+    """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_stop_publisher = rospy.Publisher('stop_music', Bool, queue_size=10)
+        rospy.sleep(0.5)  # Wait for publisher initialization
+    def play_audio(self, file_name: str, volume: int = 100, 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
+        """
+        try:
+            # Wait for service availability
+            rospy.wait_for_service('play_music', timeout=2.0)
+            # Create service client
+            play_music_service = rospy.ServiceProxy('play_music', playmusic)
+            # Call service
+            request = playmusicRequest()
+            request.music_number = file_name
+            volume = min(max(volume , 0), 100)
+            request.volume = volume
+            response = play_music_service(request)
+            SDKLogger.info(f"[Robot Audio] Requested to play audio file: {file_name}")
+            return True
+        except rospy.ROSException as e:
+            SDKLogger.error(f"[Robot Audio] Audio playback service unavailable: {str(e)}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to play audio file: {str(e)}")
+            return False
+    
+    def stop_audio(self):
+        """Stop the currently playing audio."""
+        try:
+            msg = Bool()
+            msg.data = True
+            self._audio_stop_publisher.publish(msg)
+            SDKLogger.info("[Robot Audio] Requested to stop audio playback")
+            return True 
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to stop audio playback: {str(e)}")
+            return False    
+    
+    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
+        """
+        try:
+            # Wait for service availability
+            rospy.wait_for_service('play_music', timeout=2.0)
+            # Create service client
+            play_music_service = rospy.ServiceProxy('speech_synthesis', SpeechSynthesis)
+            # Call service
+            request = SpeechSynthesisRequest()
+            request.data = text
+            request.volume = volume
+            response = play_music_service(request)
+            SDKLogger.info(f"[Robot Audio] Requested to play audio text: {text}")
+            return True
+        except rospy.ROSException as e:
+            SDKLogger.error(f"[Robot Audio] Audio playback service unavailable: {str(e)}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to play audio text: {str(e)}")
+            return False
+