kuavo-humanoid-sdk 1.1.6b1125__20250611053838-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros_env.py

@@ -0,0 +1,460 @@
+#!/usr/bin/env python3
+# coding: utf-8
+
+import os
+try:
+    import rospy
+except ImportError:
+    pass
+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.
+        """
+        # if generate docs, skip init.
+        if 'GEN_KUAVO_HUMANOID_SDK_DOCS' in os.environ:
+            return True
+
+        # 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:
+            print(f"\033[31m\nError:Can't connect to ros master, Please start roscore first or check ROS_MASTER_URI.\nException:{e}\033[0m"
+                  "\nMaybe manually launch the app first?"
+                  "\n - for example(sim): roslaunch humanoid_controller load_kuavo_mujoco_sim.launch, "
+                  "\n - for example(real): roslaunch humanoid_controller load_kuavo_real.launch\n")
+            exit(1)
+
+        """
+        # NOTE: We add kuavo_msgs/motion_capture_ik package in SDK integration.
+        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)
+        
+        # Only check nodes exist when Init SDK, if not, tips user manually launch nodes.
+        # self.launch_ik_node()
+        # self.launch_gait_switch_node()
+        deps_nodes = ['/humanoid_gait_switch_by_name']
+        for node in deps_nodes:
+            if not KuavoROSEnv.check_rosnode_exists(node):
+                print(f"\033[31m\nError:Node {node} not found. Please launch it manuallly.\033[0m")
+                exit(1)
+                
+        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}")
+        
+    
+    @staticmethod
+    def check_rosnode_exists(node_name):
+        """Check if a ROS node is running.
+        
+        Args:
+            node_name (str): Name of the node to check
+            
+        Returns:
+            bool: True if node is running, False otherwise
+        """
+        try:
+            nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+            return node_name in nodes
+        except subprocess.CalledProcessError as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False
+
+class KuavoROSEnvWebsocket:
+    _instance = None
+    _processes = []  # Store all subprocess instances
+
+    def __new__(cls):
+        if cls._instance is None:
+            cls._instance = super(KuavoROSEnvWebsocket, 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
+            self.websocket = None
+
+    @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:
+        # For WebSocket version, we'll use environment variable instead of ROS param
+        model_path = os.environ.get('KUAVO_MODEL_PATH')
+        if not model_path:
+            raise Exception("KUAVO_MODEL_PATH environment variable not found")
+        
+        if not os.path.exists(model_path):
+            raise Exception(f"Model path {model_path} not found")
+            
+        # ws 
+        return model_path.replace('/src/kuavo_assets/models', '')
+    
+    def Init(self) -> bool:
+        """
+        Initialize the WebSocket environment.
+        Raises:
+            Exception: If the KUAVO_MODEL_PATH environment variable is not found or the path is not valid.
+        """
+        # if generate docs, skip init.
+        if 'GEN_KUAVO_HUMANOID_SDK_DOCS' in os.environ:
+            return True
+
+        # Return directly if already initialized successfully
+        if self._init_success:
+            return True
+        
+        # Check if WebSocket server is running
+        try:
+            
+            self.websocket = WebSocketKuavoSDK()
+            if not self.websocket.client.is_connected:
+                print(f"\033[31m\nError: Can't connect to WebSocket server. Please ensure the server is running.\033[0m"
+                      "\nMaybe manually launch the app first?"
+                      "\n - for example(sim): roslaunch humanoid_controller load_kuavo_mujoco_sim.launch, "
+                      "\n - for example(real): roslaunch humanoid_controller load_kuavo_real.launch\n")
+                exit(1)
+        except Exception as e:
+            print(f"\033[31m\nError: Failed to connect to WebSocket server: {e}\033[0m")
+            exit(1)
+        
+        # Only check nodes exist when Init SDK, if not, tips user manually launch nodes.
+        deps_nodes = ['/humanoid_gait_switch_by_name']
+        for node in deps_nodes:
+            if not self.check_rosnode_exists(node):
+                print(f"\033[31m\nError: Node {node} not found. Please launch it manually.\033[0m")
+                exit(1)
+                
+        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:
+            if not self.websocket or not self.websocket.client.is_connected:
+                raise Exception("WebSocket server is not running")
+        except Exception as e:
+            raise Exception(f"WebSocket server is not running: {e}")
+
+        # Check if IK node and services exist
+        try:
+            # Check if arms_ik_node is running using roslibpy
+            service = roslibpy.Service(self.websocket.client, '/rosnode/list', 'rosapi/Nodes')
+            response = service.call({})
+            nodes = response.get('nodes', [])
+            
+            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 environment variable
+                robot_version = os.environ.get('ROBOT_VERSION')
+                if robot_version is None:
+                    raise Exception("Failed to get ROBOT_VERSION from environment variables")
+                
+                # 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 using roslibpy
+            service = roslibpy.Service(self.websocket.client, '/rosnode/list', 'rosapi/Nodes')
+            response = service.call({})
+            nodes = response.get('nodes', [])
+            
+            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}")
+        
+    @staticmethod
+    def check_rosnode_exists(node_name):
+        """Check if a ROS node is running using roslibpy.
+        
+        Args:
+            node_name (str): Name of the node to check
+            
+        Returns:
+            bool: True if node is running, False otherwise
+        """
+        websocket = WebSocketKuavoSDK()
+        try:
+            if not websocket or not websocket.client.is_connected:
+                return False
+                
+            service = roslibpy.Service(websocket.client, '/rosapi/nodes', 'rosapi/Nodes')
+            response = service.call({})
+            nodes = response.get('nodes', [])
+            return node_name in nodes
+        except Exception as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False

kuavo_humanoid_sdk/kuavo/dexterous_hand.py

@@ -0,0 +1,201 @@
+#!/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, KuavoDexHandTouchState
+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:
+        """控制灵巧手的位置。
+
+        Args:
+            target_positions (list): 所有手指关节的目标位置列表，长度必须为12（每只手6个手指关节），范围 => [0.0 ~ 100.0] 
+            target_velocities (list, optional): 不支持。默认为None。
+            target_torques (list, optional): 不支持。默认为None。
+
+        Returns:
+            bool: 如果控制成功返回 True，否则返回 False。
+
+        Note:
+            target_velocities 和 target_torques 参数暂不支持。
+        """
+        if len(target_positions) != self.joint_count():
+            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {self.joint_count()}")
+        
+        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:
+        """控制右手灵巧手。
+
+        Args:
+            target_positions (list): 右手关节的目标位置 [0 ~ 100]，长度必须为6
+            target_velocities (list, optional): 不支持。默认为None。
+            target_torques (list, optional): 不支持。默认为None。
+
+        Returns:
+            bool: 如果控制成功返回True，否则返回False。
+
+        Raises:
+            ValueError: 如果目标位置长度与关节数不匹配或值超出[0,100]范围
+
+        Note:
+            target_velocities 和 target_torques 参数暂不支持。
+        """
+        if len(target_positions) != (self.joint_count()/2):
+                raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {self.joint_count()/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:
+        """控制左手灵巧手。
+
+        Args:
+            target_positions (list): 左手关节的目标位置 [0 ~ 100]，长度必须为6
+            target_velocities (list, optional): 不支持。默认为None。
+            target_torques (list, optional): 不支持。默认为None。
+
+        Returns:
+            bool: 如果控制成功返回True，否则返回False。
+
+        Raises:
+            ValueError: 如果目标位置长度与关节数不匹配或值超出[0,100]范围
+
+        Note:
+            target_velocities 和 target_torques 参数不支持。
+        """
+        if len(target_positions) != (self.joint_count()/2):
+            raise ValueError(f"Target positions must have the same length as joint names {len(target_positions)} != {self.joint_count()/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:
+        """通过将所有关节位置设置为 0 来张开灵巧手。
+
+        Args:
+            side (EndEffectorSide, optional): 要打开的手。默认为 :attr:`EndEffectorSide.BOTH`。 \n
+                可以是 :attr:`EndEffectorSide.LEFT`、:attr:`EndEffectorSide.RIGHT` 或 :attr:`EndEffectorSide.BOTH`。
+
+        Returns:
+            bool: 如果打开命令发送成功返回True，否则返回False。
+        """
+        zero_pos = [0]*self.joint_count()
+        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:
+        """为双手做预定义的手势。
+
+        Args:
+            l_gesture_name (str): 左手手势的名称。None表示跳过左手。
+            r_gesture_name (str): 右手手势的名称。None表示跳过右手。
+
+        Returns:
+            bool: 如果手势命令发送成功返回True，否则返回False。
+
+        Note:
+            手势示例：'fist'、'ok'、'thumbs_up'、'666'等...
+        """
+        gesture = []
+        if l_gesture_name is not None:
+            gesture.append({'gesture_name':l_gesture_name, 'hand_side':EndEffectorSide.LEFT})
+            self.dex_hand_control.make_gestures(gesture)
+        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)
+        return True
+    def get_gesture_names(self)->list:
+        """获取所有手势的名称。
+
+        Returns:
+            list: 手势名称列表。
+                例如：['fist', 'ok', 'thumbs_up', '666', 'number_1', 'number_2', 'number_3', ... ], 如果没有手势则返回 None。
+        """
+        return self.dex_hand_control.get_gesture_names()
+    
+    def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
+        """获取灵巧手的状态。
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: 灵巧手的状态。
+        """
+        return self._rb_state.eef_state
+
+    def get_position(self)->Tuple[list, list]:
+        """获取灵巧手的位置。
+
+        Returns:
+            Tuple[list, list]: 灵巧手的位置。
+        """
+        state = self._rb_state.eef_state
+        return (state[0].position, state[1].position)
+    
+    def get_velocity(self)->Tuple[list, list]:
+        """获取灵巧手的速度。
+
+        Returns:
+            Tuple[list, list]: 灵巧手的速度。
+        """
+        state = self._rb_state.eef_state
+        return (state[0].velocity, state[1].velocity)
+
+    def get_effort(self)->Tuple[list, list]:
+        """获取灵巧手的力。
+
+        Returns:
+            Tuple[list, list]: 灵巧手的力。
+
+        Note:
+            每个手指的范围为0 ~ 100。表示最大电机电流的分数，绝对数值。
+            最大电机电流为600mA，换句话说，100。
+        """
+        state = self._rb_state.eef_state
+        return (state[0].effort, state[1].effort)
+
+    def get_grasping_state(self)->Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]:
+        """获取灵巧手的抓取状态。
+
+        Note:
+            该功能尚未实现。
+
+        Returns:
+            Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]: 灵巧手的抓取状态。
+        """
+        raise NotImplementedError("This function is not implemented yet")
+
+
+class TouchDexterousHand(DexterousHand):
+    """触觉灵巧手控制类，继承自普通灵巧手控制类，可调用普通灵巧手控制类中的所有方法"""
+    def __init__(self):
+        super().__init__()
+
+    def get_touch_state(self)-> Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]:
+        """获取灵巧手的触觉状态。
+
+        Warning:
+            该功能仅在触觉灵巧手上可用。
+
+        Returns:
+            Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]
+        """
+        return self._rb_state.dexhand_touch_state