kuavo-humanoid-sdk 0.1.2__py3-none-any.whl → 1.1.2a692__py3-none-any.whl

kuavo_humanoid_sdk/__init__.py

@@ -1,3 +1,4 @@
+from .msg import *
 from .interfaces import *
 from .kuavo import *
 from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging

kuavo_humanoid_sdk/interfaces/data_types.py

@@ -65,13 +65,13 @@ class EndEffectorState:
     """Data class representing the state of the end effector.
 
     Args:
-        position (float): Position of the end effector, [0, 100]
-        velocity (float): ...
-        effort (float): ...
+        position (list): float, Position of the end effector, range: [0, 100]
+        velocity (list): float, ...
+        effort (list): float, ...
     """
-    position: float
-    velocity: float
-    effort: float
+    position: list
+    velocity: list
+    effort: list
     class GraspingState(Enum):
         """Enum class representing the grasping states of the end effector.
 
@@ -119,4 +119,27 @@ class KuavoIKParams:
     # constraint and cost params
     oritation_constraint_tol: float = 1e-3
     pos_constraint_tol: float = 1e-3 # 0.001m, work when pos_cost_weight==0.0
-    pos_cost_weight: float = 0.0 # If U need high accuracy, set this to 0.0 !!!
+    pos_cost_weight: float = 0.0 # If U need high accuracy, set this to 0.0 !!!
+
+@dataclass
+class KuavoDexHandTouchState:
+    """Data class representing the touch state of the dexterous hand."""
+    
+    @dataclass
+    class KuavoTouchState:
+        """Data class representing the touch state of the dexterous hand."""
+        normal_force1: int  # 法向力1
+        normal_force2: int  # 法向力2
+        normal_force3: int  # 法向力3
+        tangential_force1: int  # 切向力1
+        tangential_force2: int  # 切向力2
+        tangential_force3: int  # 切向力3
+        tangential_direction1: int  # 切向力方向1
+        tangential_direction2: int  # 切向力方向2
+        tangential_direction3: int  # 切向力方向3
+        self_proximity1: int  # 自电容接近传感器1
+        self_proximity2: int  # 自电容接近传感器2
+        mutual_proximity: int  # 互电容接近传感器
+        status: int  # 传感器状态
+    # 5 fingers
+    data: Tuple[KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState]

kuavo_humanoid_sdk/interfaces/end_effector.py

@@ -4,8 +4,27 @@ from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffecto
     
 class EndEffector(ABC):
     def __init__(self, joint_names: list):
-        self.joint_names = joint_names
+        self._joint_names = joint_names
+
+    def joint_names(self)->list:
+        """Returns the joint names of the end effector.
+        
+        Returns:
+            list: The joint names of the end effector.
+        """
+        return self._joint_names
+    
+    def joint_count(self) -> int:
+        """Returns the total number of joints in the end effector.
         
+        The joint_names list contains joints for both left and right end effectors.
+        For a single end effector, the number of joints would be joint_names.size/2.
+        
+        Returns:
+            int: The total number of joints in the end effector.
+        """
+        return len(self._joint_names)
+    
     @abstractmethod
     def control(self, target_positions:list, target_velocities:list, target_torques:list)->bool:
         pass
@@ -27,15 +46,15 @@ class EndEffector(ABC):
         pass
 
     @abstractmethod
-    def get_position(self)->Tuple[float, float]:
+    def get_position(self)->Tuple[list, list]:
         pass
 
     @abstractmethod
-    def get_velocity(self)->Tuple[float, float]:
+    def get_velocity(self)->Tuple[list, list]:
         pass
 
     @abstractmethod
-    def get_effort(self)->Tuple[float, float]:
+    def get_effort(self)->Tuple[list, list]:
         pass
 
     @abstractmethod

kuavo_humanoid_sdk/kuavo/__init__.py

@@ -3,5 +3,5 @@ from .robot_info import KuavoRobotInfo
 from .robot_state import KuavoRobotState
 from .robot_arm import KuavoRobotArm
 from .robot_head import KuavoRobotHead
-from .dexterous_hand import DexterousHand
+from .dexterous_hand import DexterousHand, TouchDexterousHand
 from .leju_claw import LejuClaw

kuavo_humanoid_sdk/kuavo/core/core.py

@@ -28,7 +28,7 @@ from transitions import Machine, State
 from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose
 from kuavo_humanoid_sdk.kuavo.core.ros.control import KuavoRobotControl
 from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
-from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, kuavo_ros_param
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param
 from kuavo_humanoid_sdk.common.logger import SDKLogger
 
 # Define robot states
@@ -78,7 +78,6 @@ class KuavoRobotCore:
          raise RuntimeError if initialize failed.
         """
         try:
-            info = make_robot_param()
             # init state by gait_name
             gait_name = self._rb_state.gait_name()
             if gait_name is not None:
@@ -173,15 +172,20 @@ class KuavoRobotCore:
             SDKLogger.warn(f"[Core] control torso height failed, robot is not in stance state({self.state})!")
             return False
         
-        # Limit height range to [-0.3, 0.0]
-        limited_height = min(0.0, max(-0.3, height))
-        if height > 0.0 or height < -0.3:
-            SDKLogger.warn(f"[Core] height {height} exceeds limit [-0.3, 0.0], will be limited")
+        MIN_HEIGHT = -0.35
+        MAX_HEIGHT = 0.0
+        MIN_PITCH = -0.4
+        MAX_PITCH = 0.4
         
-        # Limit pitch range to [-0.4, 0.4]
-        limited_pitch = min(0.4, max(-0.4, pitch))
-        if abs(pitch) > 0.4:
-            SDKLogger.warn(f"[Core] pitch {pitch} exceeds limit [-0.4, 0.4], will be limited")
+        # Limit height range
+        limited_height = min(MAX_HEIGHT, max(MIN_HEIGHT, height))
+        if height > MAX_HEIGHT or height < MIN_HEIGHT:
+            SDKLogger.warn(f"[Core] height {height} exceeds limit [{MIN_HEIGHT}, {MAX_HEIGHT}], will be limited")
+        
+        # Limit pitch range
+        limited_pitch = min(MAX_PITCH, max(MIN_PITCH, pitch))
+        if abs(pitch) > MAX_PITCH:
+            SDKLogger.warn(f"[Core] pitch {pitch} exceeds limit [{MIN_PITCH}, {MAX_PITCH}], will be limited")
 
         return self._control.control_torso_height(limited_height, limited_pitch)
 
@@ -238,9 +242,9 @@ class KuavoRobotCore:
             max_y_step = 0.20
             max_yaw_step = 90
         else:
-            max_x_step = 0.10
-            max_y_step = 0.10
-            max_yaw_step = 30
+            max_x_step = 0.15
+            max_y_step = 0.15
+            max_yaw_step = 45
         
         body_poses = []
         
@@ -294,6 +298,10 @@ class KuavoRobotCore:
     def control_robot_dexhand(self, left_position:list, right_position:list)->bool:
         return self._control.control_robot_dexhand(left_position, right_position)
     
+    def robot_dexhand_command(self, data, ctrl_mode, hand_side):
+         return self._control.robot_dexhand_command(data, ctrl_mode, hand_side)
+
+
     def control_leju_claw(self, postions:list, velocities:list=[90, 90], torques:list=[1.0, 1.0]) ->bool:
         return self._control.control_leju_claw(postions, velocities, torques)
 

kuavo_humanoid_sdk/kuavo/core/dex_hand_control.py

@@ -4,6 +4,7 @@ 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
 
@@ -19,6 +20,8 @@ class DexHandControl:
             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
@@ -26,6 +29,9 @@ class DexHandControl:
     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.
@@ -60,26 +66,49 @@ class DexHandControl:
             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 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:]
-
-                # task done.
+                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/ros/control.py

@@ -1,33 +1,44 @@
-
+import os
 import rospy
 import numpy as np
 from typing import Tuple
 from kuavo_humanoid_sdk.common.logger import SDKLogger
 from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose
 from kuavo_humanoid_sdk.kuavo.core.ros.sat_utils import RotatingRectangle
-
+from kuavo_humanoid_sdk.kuavo.core.ros.param import EndEffectorType
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import JointState, Joy
-from kuavo_msgs.msg import gestureTask,robotHandPosition, robotHeadMotionData, armTargetPoses, switchGaitByName
-from kuavo_msgs.msg import footPose, footPoseTargetTrajectories
-from kuavo_msgs.srv import (gestureExecute, gestureExecuteRequest,gestureList, gestureListRequest,
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import (gestureTask,robotHandPosition, robotHeadMotionData, armTargetPoses, switchGaitByName,
+                            footPose, footPoseTargetTrajectories, dexhandCommand)                         
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import (gestureExecute, gestureExecuteRequest,gestureList, gestureListRequest,
                             controlLejuClaw, controlLejuClawRequest, changeArmCtrlMode, changeArmCtrlModeRequest)
-from motion_capture_ik.msg import twoArmHandPoseCmd, ikSolveParam
-from motion_capture_ik.srv import twoArmHandPoseCmdSrv, fkSrv
+from kuavo_humanoid_sdk.msg.motion_capture_ik.msg import twoArmHandPoseCmd, ikSolveParam
+from kuavo_humanoid_sdk.msg.motion_capture_ik.srv import twoArmHandPoseCmdSrv, fkSrv
 
 class ControlEndEffector:
-    def __init__(self, eef_type: str = "qiangnao"):
-        self._pubs = []
+    def __init__(self, eef_type: str = EndEffectorType.QIANGNAO):
         self._eef_type = eef_type
-        if eef_type == "qiangnao":
+        self._pubs = []
+        if self._eef_type == EndEffectorType.QIANGNAO:
+            self._pub_ctrl_robot_hand = rospy.Publisher('/control_robot_hand_position', robotHandPosition, queue_size=10)                
+            # publisher, name, require
+            self._pubs.append((self._pub_ctrl_robot_hand, True))
+        elif self._eef_type == EndEffectorType.QIANGNAO_TOUCH:
             self._pub_ctrl_robot_hand = rospy.Publisher('/control_robot_hand_position', robotHandPosition, queue_size=10)                
-            self._pubs.append(self._pub_ctrl_robot_hand)
+            self._pub_dexhand_command = rospy.Publisher('/dexhand/command', dexhandCommand, queue_size=10)
+            self._pub_dexhand_right_command = rospy.Publisher('/dexhand/right/command', dexhandCommand, queue_size=10)
+            self._pub_dexhand_left_command = rospy.Publisher('/dexhand/left/command', dexhandCommand, queue_size=10)
+            # publisher, name, require
+            self._pubs.append((self._pub_dexhand_command, True))
+            self._pubs.append((self._pub_dexhand_right_command, True))
+            self._pubs.append((self._pub_dexhand_left_command, True))
+            
 
     def connect(self, timeout:float=1.0)-> bool:
         start_time = rospy.Time.now()
         
         success = True
-        for pub in self._pubs:
+        for pub, require in self._pubs:
             while pub.get_num_connections() == 0:
                 if (rospy.Time.now() - start_time).to_sec() > timeout:
                     SDKLogger.error(f"Timeout waiting for {pub.name} connection")
@@ -39,7 +50,7 @@ class ControlEndEffector:
 
     """ Control Kuavo Robot Dexhand """
     def pub_control_robot_dexhand(self, left_position:list, right_position:list)->bool:
-        if self._eef_type != 'qiangnao':
+        if not self._eef_type.startswith(EndEffectorType.QIANGNAO): # qiangnao, qiangnao_touch
             SDKLogger.warning(f"{self._eef_type} not support control dexhand")
             return False
         try :
@@ -52,9 +63,41 @@ class ControlEndEffector:
         except Exception as e:
             SDKLogger.error(f"publish robot dexhand: {e}")
             return False
+    
+    def pub_dexhand_command(self, data:list, ctrl_mode, hand_side)->bool:
+        """
+            ctrl_mode: 0 --> POSITION, 1 --> VELOCITY
+            hand_side: 0 --> left, 1 --> right, 2-->dual
+        """
+        if not self._eef_type == EndEffectorType.QIANGNAO_TOUCH:
+            SDKLogger.warning(f"{self._eef_type} not support pub_left_dexhand_command")
+            return False
+        try:
+            if hand_side != 2 and len(data) != 6:
+                SDKLogger.warning("Data length should be 6")
+                return False
+            if hand_side == 2 and len(data) != 12:
+                SDKLogger.warning("Data length should be 12")
+                return False
+            if ctrl_mode not in [dexhandCommand.POSITION_CONTROL, dexhandCommand.VELOCITY_CONTROL]:
+                SDKLogger.error(f"Invalid mode for pub_left_dexhand_command: {ctrl_mode}")
+                return False
+            msg = dexhandCommand()
+            msg.data = [int(d) for d in data]  # Convert data to integers
+            msg.control_mode = ctrl_mode
+            if hand_side == 0:
+                self._pub_dexhand_left_command.publish(msg)
+            elif hand_side == 1:
+                self._pub_dexhand_right_command.publish(msg)
+            else:
+                self._pub_dexhand_command.publish(msg)
+            return True        
+        except Exception as e:
+            SDKLogger.error(f"Failed to publish left dexhand command: {e}")
+            return False
         
     def srv_execute_gesture(self, gestures:list)->bool:
-        if self._eef_type != 'qiangnao':
+        if not self._eef_type.startswith(EndEffectorType.QIANGNAO): # qiangnao, qiangnao_touch
             SDKLogger.warning(f"{self._eef_type} not support control dexhand")
             return False
         try:
@@ -83,7 +126,7 @@ class ControlEndEffector:
         return False
 
     def srv_get_gesture_names(self)->list:
-        if self._eef_type != 'qiangnao':
+        if not self._eef_type.startswith(EndEffectorType.QIANGNAO): # qiangnao, qiangnao_touch
             SDKLogger.warning(f"{self._eef_type} not support control dexhand")
             return []
         try:
@@ -264,7 +307,7 @@ class ControlRobotMotion:
         self._pub_switch_gait = rospy.Publisher('/humanoid_switch_gait_by_name', switchGaitByName, queue_size=10)
         self._pub_step_ctrl = rospy.Publisher('/humanoid_mpc_foot_pose_target_trajectories', footPoseTargetTrajectories, queue_size=10)
 
-    def connect(self, timeout:float=1.0)-> bool:
+    def connect(self, timeout:float=2.0)-> bool:
         start_time = rospy.Time.now()
         publishers = [
             # (self._pub_joy, "joy publisher"),
@@ -510,6 +553,19 @@ class KuavoRobotControl:
         SDKLogger.debug(f"Control robot dexhand: {left_position}, {right_position}")
         return self.kuavo_eef_control.pub_control_robot_dexhand(left_position, right_position)
 
+    def robot_dexhand_command(self, data, ctrl_mode, hand_side):
+        """
+            Publish dexhand command
+            Args:
+                - data: list of 6 floats between 0 and 100
+                - ctrl_mode: int between 0(position), 1(velocity)
+                - hand_side: int between 0(left), 1(right), 2(dual)
+        """
+        if self.kuavo_eef_control is None:
+            SDKLogger.error("End effector control is not initialized.")
+            return False
+        return self.kuavo_eef_control.pub_dexhand_command(data, ctrl_mode, hand_side)
+
     def execute_gesture(self, gestures:list)->bool:
         """
             Execute gestures

kuavo_humanoid_sdk/kuavo/core/ros/param.py

@@ -2,6 +2,12 @@ import rospy
 import json
 import xml.etree.ElementTree as ET
 from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+# End effector types
+class EndEffectorType:
+    QIANGNAO = "qiangnao"
+    QIANGNAO_TOUCH = "qiangnao_touch"
+    LEJUCLAW = "lejuclaw"
 class RosParameter:
     def __init__(self):
         pass

kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -4,12 +4,13 @@ import time
 from typing import Tuple
 from std_msgs.msg import Float64
 from nav_msgs.msg import Odometry
-from kuavo_msgs.msg import sensorsData, lejuClawState, gaitTimeName
-
+from sensor_msgs.msg import JointState
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import sensorsData, lejuClawState, gaitTimeName, dexhandTouchState
 from kuavo_msgs.srv import changeArmCtrlMode, changeArmCtrlModeRequest, getCurrentGaitName, getCurrentGaitNameRequest
-from ocs2_msgs.msg import mpc_observation
-from kuavo_humanoid_sdk.interfaces.data_types import KuavoImuData, KuavoJointData, KuavoOdometry, KuavoArmCtrlMode, EndEffectorState
-from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param
+from kuavo_humanoid_sdk.msg.ocs2_msgs.msg import mpc_observation
+from kuavo_humanoid_sdk.interfaces.data_types import (KuavoImuData, KuavoJointData, KuavoOdometry, 
+                                                      KuavoArmCtrlMode, EndEffectorState, KuavoDexHandTouchState)
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
 from kuavo_humanoid_sdk.common.logger import SDKLogger
 
 from collections import deque
@@ -66,12 +67,32 @@ class KuavoRobotStateCore:
             rospy.Subscriber("/humanoid_mpc_observation", mpc_observation, self._humanoid_mpc_observation_callback)
             
             kuavo_info = make_robot_param()
-            if kuavo_info['end_effector_type'] == "lejuclaw":
+            self._ee_type = kuavo_info['end_effector_type']
+            if self._ee_type == EndEffectorType.LEJUCLAW:
                 rospy.Subscriber("/leju_claw_state", lejuClawState, self._lejuclaw_state_callback)
-            elif kuavo_info['end_effector_type'] == "qiangnao":
-                pass # TODO(kuavo): add qiangnao state subscriber
-                # rospy.Subscriber("/robot_hand_state", robotHandState, self._dexterous_hand_state_callback)
-            
+            elif self._ee_type == EndEffectorType.QIANGNAO:
+                rospy.Subscriber("/dexhand/state", JointState, self._dexterous_hand_state_callback)
+            elif self._ee_type == EndEffectorType.QIANGNAO_TOUCH:
+                rospy.Subscriber("/dexhand/state", JointState, self._dexterous_hand_state_callback)
+                rospy.Subscriber("/dexhand/touch_state", dexhandTouchState, self._dexhand_touch_state_callback)
+                # Initialize touch state for both hands with empty KuavoDexHandTouchState instances
+                self._dexhand_touch_state = (
+                    KuavoDexHandTouchState(
+                        data=(KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0))
+                    ),  # Left hand touch state
+                    KuavoDexHandTouchState(
+                        data=(KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
+                              KuavoDexHandTouchState.KuavoTouchState(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0))
+                    )   # Right hand touch state
+                )
+
             """ data """
             self._terrain_height = 0.0
             self._imu_data = KuavoImuData(
@@ -93,17 +114,17 @@ class KuavoRobotStateCore:
                 angular = (0.0, 0.0, 0.0)
             )
             self._eef_state = (EndEffectorState(
-                position = 0.0,
-                velocity = 0.0,
-                effort = 0.0,
+                position = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                velocity = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                effort = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
                 state=EndEffectorState.GraspingState.UNKNOWN
             ), EndEffectorState(
-                position = 0.0,
-                velocity = 0.0,
-                effort = 0.0,
+                position = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                velocity = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                effort = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
                 state=EndEffectorState.GraspingState.UNKNOWN
             ))
-
+                            
             # gait manager
             self._gait_manager = GaitManager()
             self._prev_gait_name = self.gait_name
@@ -156,6 +177,12 @@ class KuavoRobotStateCore:
     def eef_state(self)->Tuple[EndEffectorState, EndEffectorState]:
         return self._eef_state
     
+    @property
+    def dexhand_touch_state(self)-> Tuple[KuavoDexHandTouchState, KuavoDexHandTouchState]:
+        if self._ee_type != EndEffectorType.QIANGNAO_TOUCH:
+            raise Exception("This robot does not support touch state")
+        return self._dexhand_touch_state
+
     @property
     def current_observation_time(self) -> float:
         """Get the current observation time from MPC.
@@ -231,7 +258,7 @@ class KuavoRobotStateCore:
             position = copy.deepcopy(msg.joint_data.joint_q),
             velocity = copy.deepcopy(msg.joint_data.joint_v),
             torque = copy.deepcopy(msg.joint_data.joint_vd),
-            acceleration = copy.deepcopy(msg.joint_data.joint_current)
+            acceleration = copy.deepcopy(msg.joint_data.joint_current if hasattr(msg.joint_data, 'joint_current') else msg.joint_data.joint_torque)
         )
 
     def _odom_callback(self, msg:Odometry)->None:
@@ -245,29 +272,57 @@ class KuavoRobotStateCore:
 
     def _lejuclaw_state_callback(self, msg:lejuClawState)->None:
         self._eef_state = (EndEffectorState(
-            position = msg.data.position[0],
-            velocity = msg.data.velocity[0],
-            effort = msg.data.effort[0],
+            # left claw
+            position = [msg.data.position[0]],
+            velocity = [msg.data.velocity[0]],
+            effort = [msg.data.effort[0]],
             state=EndEffectorState.GraspingState(msg.state[0])
         ), EndEffectorState(
-            position = msg.data.position[1],
-            velocity = msg.data.velocity[1],
-            effort = msg.data.effort[1],
+            # right claw
+            position = [msg.data.position[1]],
+            velocity = [msg.data.velocity[1]],
+            effort = [msg.data.effort[1]],
             state=EndEffectorState.GraspingState(msg.state[1])
         ))
 
-    def _dexterous_hand_state_callback(self, msg)->None:
+    def _dexterous_hand_state_callback(self, msg:JointState)->None:
         self._eef_state = (EndEffectorState(
-            position = msg.data.position[0],
-            velocity = msg.data.velocity[0],
-            effort = msg.data.effort[0],
-            state=EndEffectorState.GraspingState(msg.state[0])
+            # left hand
+            position = list(msg.position[:len(msg.position)//2]),
+            velocity = list(msg.velocity[:len(msg.velocity)//2]),
+            effort = list(msg.effort[:len(msg.effort)//2]),
+            state=EndEffectorState.GraspingState.UNKNOWN
         ), EndEffectorState(
-            position = msg.data.position[1],
-            velocity = msg.data.velocity[1],
-            effort = msg.data.effort[1],
-            state=EndEffectorState.GraspingState(msg.state[1])
+            # right hand
+            position = list(msg.position[len(msg.position)//2:]),
+            velocity = list(msg.velocity[len(msg.velocity)//2:]),
+            effort = list(msg.effort[len(msg.effort)//2:]),
+            state=EndEffectorState.GraspingState.UNKNOWN
         ))
+
+    def _dexhand_touch_state_callback(self, msg:dexhandTouchState)->None:
+        # Update touch state for both hands
+        self._dexhand_touch_state = (
+            KuavoDexHandTouchState(
+                data=tuple(KuavoDexHandTouchState.KuavoTouchState(
+                    sensor.normal_force1, sensor.normal_force2, sensor.normal_force3,
+                    sensor.tangential_force1, sensor.tangential_force2, sensor.tangential_force3,
+                    sensor.tangential_direction1, sensor.tangential_direction2, sensor.tangential_direction3,
+                    sensor.self_proximity1, sensor.self_proximity2, sensor.mutual_proximity,
+                    sensor.status
+                ) for sensor in msg.left_hand)
+            ),  # Left hand touch state
+            KuavoDexHandTouchState(
+                data=tuple(KuavoDexHandTouchState.KuavoTouchState(
+                    sensor.normal_force1, sensor.normal_force2, sensor.normal_force3,
+                    sensor.tangential_force1, sensor.tangential_force2, sensor.tangential_force3,
+                    sensor.tangential_direction1, sensor.tangential_direction2, sensor.tangential_direction3,
+                    sensor.self_proximity1, sensor.self_proximity2, sensor.mutual_proximity,
+                    sensor.status
+                ) for sensor in msg.right_hand)
+            )   # Right hand touch state
+        )
+
     def _humanoid_mpc_gait_changed_callback(self, msg: gaitTimeName):
         """
         Callback function for gait change messages.