kuavo-humanoid-sdk 0.1.0__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros_env.py

@@ -0,0 +1,196 @@
+#!/usr/bin/env python3
+# coding: utf-8
+
+import os
+import rospy
+import subprocess
+import atexit
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+class KuavoROSEnv:
+    _instance = None
+    _processes = []  # Store all subprocess instances
+
+    def __new__(cls):
+        if cls._instance is None:
+            cls._instance = super(KuavoROSEnv, cls).__new__(cls)
+            cls._instance._initialized = False
+            cls._instance._init_success = False  # Add flag to track successful Init() call
+            # Register cleanup handler
+            atexit.register(cls._cleanup_processes)
+        return cls._instance
+
+    def __init__(self):
+        if not self._initialized:
+            self._initialized = True
+
+    @classmethod
+    def _cleanup_processes(cls):
+        """Cleanup all registered processes on exit"""
+        for process in cls._processes:
+            if process.poll() is None:  # Process is still running
+                process.terminate()
+                try:
+                    process.wait(timeout=3)  # Wait for process to terminate
+                except subprocess.TimeoutExpired:
+                    process.kill()  # Force kill if not terminated
+        cls._processes.clear()
+
+    def _get_kuavo_ws_root(self) -> str:
+        if not rospy.has_param('/modelPath'):
+            raise Exception("modelPath parameter not found")
+        model_path = rospy.get_param('/modelPath')
+        
+        if not os.path.exists(model_path):
+            raise Exception(f"modelPath f{model_path} not found")
+        # ws 
+        return model_path.replace('/src/kuavo_assets/models', '')
+    
+    def Init(self) -> bool:
+        """
+        Initialize the ROS environment.
+        Raises:
+            Exception: If the modelPath parameter is not found or the modelPath parameter is not a valid path.
+        """
+        # Return directly if already initialized successfully
+        if self._init_success:
+            return True
+
+        # Check if ROS master is running
+        try:
+            rospy.get_master().getPid()
+        except Exception as e:
+            raise Exception(f"ROS master is not running. Please start roscore first or check ROS_MASTER_URI, {e}")
+        
+        kuavo_ws_root = self._get_kuavo_ws_root()
+        
+        # Add kuavo_msgs package to Python path
+        dist_path = os.path.join(kuavo_ws_root, 'devel/lib/python3/dist-packages')
+        if not os.path.exists(dist_path):
+            dist_path = os.path.join(kuavo_ws_root, 'install/lib/python3/dist-packages')
+            if not os.path.exists(dist_path):
+                raise Exception(f"{dist_path} package not found in Python path")
+        
+        if dist_path not in os.sys.path:
+            os.sys.path.append(dist_path)
+
+        # Initialize the ROS node
+        if not rospy.get_node_uri():
+            rospy.init_node(f'kuavo_sdk_node', anonymous=True, disable_signals=True)
+        
+        self.launch_ik_node()
+        self.launch_gait_switch_node()
+        
+        self._init_success = True  # Set flag after successful initialization
+        return True
+    def _launch_ros_node(self, node_name, launch_cmd, log_name):
+        """Launch a ROS node with the given command and log the output.
+        
+        Args:
+            node_name (str): Name of the node to launch
+            launch_cmd (str): Full launch command including source and roslaunch
+            log_name (str): Name for the log file
+            
+        Raises:
+            Exception: If node launch fails
+        """
+        # Launch in background and check if successful
+        try:
+            os.makedirs('/var/log/kuavo_humanoid_sdk', exist_ok=True)
+            log_path = f'/var/log/kuavo_humanoid_sdk/{log_name}.log'
+        except PermissionError:
+            os.makedirs('log/kuavo_humanoid_sdk', exist_ok=True)
+            log_path = f'log/kuavo_humanoid_sdk/{log_name}.log'
+            
+        with open(log_path, 'w') as log_file:
+            process = subprocess.Popen(launch_cmd, shell=True, executable='/bin/bash', stdout=log_file, stderr=log_file)
+            self._processes.append(process)  # Add process to tracking list
+            
+        if process.returncode is not None and process.returncode != 0:
+            raise Exception(f"Failed to launch {node_name}, return code: {process.returncode}")
+
+        SDKLogger.info(f"{node_name} launched successfully")
+
+    def _get_setup_file(self, ws_root=None):
+        """Get the appropriate ROS setup file path based on shell type.
+        
+        Args:
+            ws_root (str, optional): ROS workspace root path. If None, uses ROS_WORKSPACE.
+            
+        Returns:
+            str: Path to the setup file
+            
+        Raises:
+            Exception: If setup file not found
+        """
+        is_zsh = 'zsh' in os.environ.get('SHELL', '')
+        
+        if ws_root is None:
+            ws_root = os.environ['ROS_WORKSPACE']
+            
+        setup_files = {
+            'zsh': os.path.join(ws_root, 'devel/setup.zsh'),
+            'bash': os.path.join(ws_root, 'devel/setup.bash')
+        }
+        
+        setup_file = setup_files['zsh'] if is_zsh else setup_files['bash']
+        if not os.path.exists(setup_file):
+            setup_file = setup_file.replace('devel', 'install')
+            if not os.path.exists(setup_file):
+                raise Exception(f"Setup file not found in either devel or install: {setup_file}")
+                
+        return setup_file
+
+    def launch_ik_node(self):
+        # nodes: /arms_ik_node
+        # services: /ik/two_arm_hand_pose_cmd_srv, /ik/fk_srv
+        try:
+            rospy.get_master().getPid()
+        except Exception as e:
+            raise Exception(f"ROS master is not running. Please start roscore first or check ROS_MASTER_URI, {e}")
+
+        # Check if IK node and services exist
+        try:
+            # Check if arms_ik_node is running
+            nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+            if '/arms_ik_node' not in nodes:
+                # Launch IK node if not running
+                kuavo_ws_root = self._get_kuavo_ws_root()
+                setup_file = self._get_setup_file(kuavo_ws_root)
+                source_cmd = f"source {setup_file}"
+                
+                # Get robot version from rosparam
+                robot_version = rospy.get_param('/robot_version', None)
+                if robot_version is None:
+                    raise Exception("Failed to get robot_version from rosparam")
+                
+                # Launch IK node
+                launch_cmd = f"roslaunch motion_capture_ik ik_node.launch robot_version:={robot_version}"
+                full_cmd = f"{source_cmd} && {launch_cmd}"
+                
+                self._launch_ros_node('IK node', full_cmd, 'launch_ik')
+
+            return True
+
+        except Exception as e:
+            raise Exception(f"Failed to verify IK node and services: {e}")
+        
+
+    def launch_gait_switch_node(self)-> bool:
+        """Verify that the gait switch node is running, launch if not."""
+        try:
+             # Check if node exists
+             nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+             if '/humanoid_gait_switch_by_name' not in nodes:
+                kuavo_ws_root = self._get_kuavo_ws_root()
+                setup_file = self._get_setup_file(kuavo_ws_root)
+                source_cmd = f"source {setup_file}"
+    
+                # Launch gait switch node
+                launch_cmd = "roslaunch humanoid_interface_ros humanoid_switch_gait.launch"
+                full_cmd = f"{source_cmd} && {launch_cmd}"
+                
+                self._launch_ros_node('Gait switch node', full_cmd, 'launch_gait_switch')
+
+        except Exception as e:
+            raise Exception(f"Failed to launch gait_switch node: {e}")

kuavo_humanoid_sdk/kuavo/dexterous_hand.py

@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+# coding: utf-8
+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.dex_hand_control import DexHandControl
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+
+class DexterousHand(EndEffector):
+    def __init__(self):
+        joint_names = ['l_thumb', 'l_thumb_aux', 'l_index', 'l_middle', 'l_ring', 'l_pinky',
+                       '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()
+
+    def control(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """Set the position of the hand.
+
+        Args:
+            target_positions (list): List of target positions for all joints, length must be 12 (6 joints for each hand),
+                range => [0.0 ~ 100.0]
+            target_velocities (list, optional): Not supported. Defaults to None.
+            target_torques (list, optional): Not supported. Defaults to None.
+
+        Returns:
+            bool: True if control successful, False otherwise.
+
+        Note:
+            target_velocities and target_torques are not supported.
+        """
+        if len(target_positions) != len(self.joint_names):
+            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {len(self.joint_names)}")
+        
+        q = [max(0, min(100, pos if isinstance(pos, int) else int(pos))) for pos in target_positions]
+
+        # control the hand
+        return self.dex_hand_control.control(target_positions=q, side=EndEffectorSide.BOTH)
+
+    def control_right(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """Control the right dexterous hand.
+
+        Args:
+            target_positions (list): Target positions for right hand joints [0 ~ 100], length must be 6
+            target_velocities (list, optional): Not supported. Defaults to None.
+            target_torques (list, optional): Not supported. Defaults to None.
+
+        Returns:
+            bool: True if control successful, False otherwise.
+
+        Raises:
+            ValueError: If target positions length doesn't match joint count or values outside [0,100] range
+
+        Note:
+            target_velocities and target_torques are not supported.
+        """
+        if len(target_positions) != (len(self.joint_names)/2):
+                raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {len(self.joint_names)/2}.")
+        
+        q = [max(0, min(100, pos if isinstance(pos, int) else int(pos))) for pos in target_positions]
+
+        return self.dex_hand_control.control(target_positions=q, side=EndEffectorSide.RIGHT)
+
+    def control_left(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """Control the left dexterous hand.
+
+        Args:
+            target_positions (list): Target positions for left hand joints [0 ~ 100], length must be 6
+            target_velocities (list, optional): Not supported. Defaults to None.
+            target_torques (list, optional): Not supported. Defaults to None.
+
+        Returns:
+            bool: True if control successful, False otherwise.
+
+        Raises:
+            ValueError: If target positions length doesn't match joint count or values outside [0,100] range
+
+        Note:
+            target_velocities and target_torques are not supported.
+        """
+        if len(target_positions) != (len(self.joint_names)/2):
+            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {len(self.joint_names)/2}.")
+        
+        q = [max(0, min(100, pos if isinstance(pos, int) else int(pos))) for pos in target_positions]
+
+        return self.dex_hand_control.control(target_positions=q, side=EndEffectorSide.LEFT)
+
+    def open(self, side: EndEffectorSide=EndEffectorSide.BOTH)->bool:
+        """Open the dexterous hand(s) by setting all joint positions to 0.
+
+        Args:
+            side (EndEffectorSide, optional): Which hand(s) to open. Defaults to EndEffectorSide.BOTH.
+                Can be LEFT, RIGHT, or BOTH.
+
+        Returns:
+            bool: True if open command sent successfully, False otherwise.
+        """
+        zero_pos = [0]*len(self.joint_names)
+        if side == EndEffectorSide.LEFT:
+            return self.dex_hand_control.control(target_positions=zero_pos[:len(zero_pos)//2], side=EndEffectorSide.LEFT)
+        elif side == EndEffectorSide.RIGHT:
+            return self.dex_hand_control.control(target_positions=zero_pos[len(zero_pos)//2:], side=EndEffectorSide.RIGHT)
+        else:
+            return self.dex_hand_control.control(target_positions=zero_pos, side=EndEffectorSide.BOTH)     
+
+    def make_gesture(self, l_gesture_name: str, r_gesture_name: str)->bool:
+        """Make predefined gestures for both hands.
+
+        Args:
+            l_gesture_name (str): Name of gesture for left hand. None to skip left hand.
+            r_gesture_name (str): Name of gesture for right hand. None to skip right hand.
+
+        Returns:
+            bool: True if gesture command sent successfully, False otherwise.
+
+        Note:
+            gestures e.g.: 'fist', 'ok', 'thumbs_up', '666'...
+        """
+        gesture = []
+        if l_gesture_name is not None:
+            gesture.append({'gesture_name':l_gesture_name, 'hand_side':EndEffectorSide.LEFT})
+        if r_gesture_name is not None:
+            gesture.append({'gesture_name':r_gesture_name, 'hand_side':EndEffectorSide.RIGHT})    
+        self.dex_hand_control.make_gestures(gesture)
+
+    def get_gesture_names(self)->list:
+        """Get the names of all gestures.
+
+        Returns:
+            list: List of gesture names.
+                e.g.: ['fist', 'ok', 'thumbs_up', '666', 'number_1', 'number_2', 'number_3', ... ],
+                None if no gestures.
+        """
+        return self.dex_hand_control.get_gesture_names()
+    
+    def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
+        # TODO(kuavo): Not implemented yet
+        """Get the state of the dexterous hand.
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: The state of the dexterous hand.
+        """
+        return self._rb_state.eef_state
+
+    def get_position(self)->Tuple[float, float]:
+        # TODO(kuavo): Not implemented yet
+        """Get the position of the dexterous hand.
+
+        Returns:
+            Tuple[float, float]: The position of the dexterous hand.
+        """
+        return self._rb_state.eef_state.position
+    
+    def get_velocity(self)->Tuple[float, float]:
+        # TODO(kuavo): Not implemented yet
+        """Get the velocity of the dexterous hand.
+
+        Returns:
+            Tuple[float, float]: The velocity of the dexterous hand.
+        """
+        return self._rb_state.eef_state.velocity
+
+    def get_effort(self)->Tuple[float, float]:
+        # TODO(kuavo): Not implemented yet
+        """Get the effort of the dexterous hand.
+
+        Returns:
+            Tuple[float, float]: The effort of the dexterous hand.
+
+        Note:
+            0 ~ 100 for each finger. Fraction of max motor current, absolute number.
+            The max motor current is 600mA, in a word, 100.
+        """
+        return self._rb_state.eef_state.effort
+
+    def get_grasping_state(self)->Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]:
+        # TODO(kuavo): Not implemented yet
+        """Get the grasping state of the dexterous hand.
+
+        Returns:
+            Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]: The grasping state of the dexterous hand.
+        """
+        return self._rb_state.eef_state.state

kuavo_humanoid_sdk/kuavo/leju_claw.py

@@ -0,0 +1,225 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import time
+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
+
+class LejuClaw(EndEffector):
+    def __init__(self):
+        super().__init__(joint_names=['left_claw', 'right_claw'])
+        self.leju_claw_control = LejuClawControl()
+        self._rb_state = KuavoRobotStateCore()
+
+    def control(self, target_positions: list, target_velocities:list=None, target_torques: list=None)->bool:
+        """Control the claws to grip.
+
+        Args:
+            target_positions (list): The target positions of the claws.
+            target_velocities (list, optional): The target velocities of the claws. If None, default value [90, 90] will be used.
+            target_torques (list, optional): The target torques of the claws. If None, default value [1.0, 1.0] will be used.
+
+        Note:
+            The target_positions, target_velocities,  target_torques must be a list of length 2.
+            After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
+
+        Warning:
+            If the claws are still in motion from a previous command, this request may be dropped.
+
+        Returns:
+            bool: True if the claws are successfully gripped, False otherwise
+        """
+        if target_positions is None:
+            raise ValueError("Target positions must be provided.")
+        
+        target_positions = [max(0.0, min(100.0, pos)) for pos in target_positions]
+        if target_velocities is None:
+            target_velocities = [90, 90]
+        else:
+           target_velocities = [max(0.0, min(100.0, vel)) for vel in target_velocities]
+        
+        if target_torques is None:
+            target_torques = [1.0, 1.0]
+        else:
+            target_torques = [max(0.0, min(10.0, torque)) for torque in target_torques]
+            
+        return self.leju_claw_control.control(target_positions, target_velocities, target_torques, EndEffectorSide.BOTH)
+
+    def control_left(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """Control the left claw to grip.
+
+        Args:
+            target_positions (list): The target position of the left claw.
+            target_velocities (list, optional): The target velocity of the left claw. If None, default value 90 will be used.
+            target_torques (list, optional): The target torque of the left claw. If None, default value 1.0 will be used.
+
+        Note:
+            The target_positions, target_velocities, target_torques must be a list of length 1
+            After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
+
+        Warning:
+            If the claws are still in motion from a previous command, this request may be dropped.
+
+        Returns:
+            bool: True if the claw is successfully gripped, False otherwise.
+        """
+        if target_positions is None:
+            raise ValueError("Target positions must be provided.")
+        
+        for data in [target_positions, target_velocities, target_torques]:
+            if data is not None and len(data) != 1:
+                raise ValueError("Target positions must be a list of length 1.")
+        
+        q = max(0.0, min(100.0, target_positions[0]))
+
+        if target_velocities is not None:
+            v = max(0.0, min(100.0, target_velocities[0]))
+        else:
+            v = 90
+
+        if target_torques is not None:
+            tau = max(0.0, min(10.0, target_torques[0]))
+        else:
+            tau = 1.0
+
+        return self.leju_claw_control.control([q], [v], [tau], EndEffectorSide.LEFT)
+
+    def control_right(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
+        """Control the right claw to grip.
+
+        Args:
+            target_positions (list): The target position of the right claw.
+            target_velocities (list, optional): The target velocity of the right claw. If None, default value 90 will be used.
+            target_torques (list, optional): The target torque of the right claw. If None, default value 1.0 will be used.
+
+        Returns:
+            bool: True if the claw is successfully gripped, False otherwise.
+        
+        Note:
+            The target_positions, target_velocities, target_torques must be a list of length 1
+            After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
+
+        Warning:
+            If the claws are still in motion from a previous command, this request may be dropped.    
+        """
+        if target_positions is None:
+            raise ValueError("Target positions must be provided.")
+        
+        for data in [target_positions, target_velocities, target_torques]:
+            if data is not None and len(data) != 1:
+                raise ValueError("Target positions must be a list of length 1.")
+        
+        q = max(0.0, min(100.0, target_positions[0]))
+
+        if target_velocities is not None:
+            v = max(0.0, min(100.0, target_velocities[0]))
+        else:
+            v = 90
+
+        if target_torques is not None:
+            tau = max(0.0, min(10.0, target_torques[0]))
+        else:
+            tau = 1.0
+
+        return self.leju_claw_control.control([q], [v], [tau], EndEffectorSide.RIGHT)
+    
+    def open(self, side:EndEffectorSide=EndEffectorSide.BOTH)->bool:
+        """Control the claws to release/open.
+
+        Note:
+            Control the claws to open.
+            After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
+
+        Args:
+            side (EndEffectorSide, optional): The side to control. Defaults to EndEffectorSide.BOTH.
+
+        Returns:
+            bool: True if the claw is successfully released, False otherwise.
+        """
+        return self.leju_claw_control.release(side)
+
+    def close(self, side:EndEffectorSide=EndEffectorSide.BOTH)->bool:
+        """Control the claws to close/grip.
+
+        Note:
+            Control the claws to close.
+            After calling this function, you can call wait_for_finish() to wait until the claws reach the target position.
+
+        Args:
+            side (EndEffectorSide, optional): The side to control. Defaults to EndEffectorSide.BOTH.
+
+        Returns:
+            bool: True if the claw is successfully gripped, False otherwise.
+        """
+        return self.leju_claw_control.control(position=[100, 100], velocity=[90, 90], torque=[1.0, 1.0], side=side)
+
+    def get_grasping_state(self)->Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]:
+        """Get the grasping state of the claws.
+
+        Returns:
+            Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]: The grasping state of the claws.
+        """
+        return self._rb_state.eef_state.state
+    
+    def get_position(self)->Tuple[float, float]:
+        """Get the position of the claws.
+
+        Returns:
+            Tuple[float, float]: The position of the claws, range [0.0, 100.0].
+        """
+        return self._rb_state.eef_state.position
+    
+    def get_velocity(self)->Tuple[float, float]:
+        """Get the velocity of the claws.
+
+        Returns:
+            Tuple[float, float]: The velocity of the claws.
+        """
+        return self._rb_state.eef_state.velocity
+    
+    def get_effort(self)->Tuple[float, float]:
+        """Get the effort of the claws.
+
+        Returns:
+            Tuple[float, float]: The effort of the claws.
+        """
+        return self._rb_state.eef_state.effort
+
+    def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
+        """Get the state of the claws.
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: The state of the claws.
+                - position: The position of the claws, range [0.0, 100.0].
+                - velocity: The velocity of the claws.
+                - effort: The effort of the claws.
+                - state: The grasping state of the claws.
+        """
+        return self._rb_state.eef_state
+
+    def wait_for_finish(self, side: EndEffectorSide=EndEffectorSide.BOTH, timeout:float=2.5):
+        """Wait for the claw motion to finish.
+
+        Args:
+            side (EndEffectorSide, optional): The side of the claw to wait for. Defaults to EndEffectorSide.BOTH.
+            timeout (float, optional): The timeout duration in seconds. Defaults to 2.5.
+
+        Returns:
+            bool: True if motion completed before timeout, False otherwise.
+        """
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            if side == EndEffectorSide.BOTH:
+                if self._rb_state.eef_state[0].state >= EndEffectorState.GraspingState.REACHED and \
+                   self._rb_state.eef_state[1].state >= EndEffectorState.GraspingState.REACHED:
+                    return True
+            elif side == EndEffectorSide.LEFT:
+                if self._rb_state.eef_state[0].state >= EndEffectorState.GraspingState.REACHED:
+                    return True
+            elif side == EndEffectorSide.RIGHT:
+                if self._rb_state.eef_state[1].state >= EndEffectorState.GraspingState.REACHED:
+                    return True
+            time.sleep(0.1)
+        return False