kuavo-humanoid-sdk 1.1.3a1252__py3-none-any.whl → 1.1.6__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros/param.py

@@ -1,12 +1,8 @@
-from kuavo_humanoid_sdk.common.global_config import GlobalConfig
-try:
-    import rospy
-except ImportError:
-    pass
+import rospy
 import json
 import xml.etree.ElementTree as ET
 from kuavo_humanoid_sdk.common.logger import SDKLogger
-from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+
 # End effector types
 class EndEffectorType:
     QIANGNAO = "qiangnao"
@@ -68,126 +64,7 @@ class RosParameter:
             return None
         return rospy.get_param('/initial_state')
 
-import roslibpy
-class RosParamWebsocket:
-    def __init__(self):
-        self.websocket = WebSocketKuavoSDK()
-        if not self.websocket.client.is_connected:
-            SDKLogger.error("Failed to connect to WebSocket server")
-            raise ConnectionError("Failed to connect to WebSocket server")
-
-    def robot_version(self)->str:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'robot_version')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("robot_version parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get robot_version: {e}")
-            return None
-    
-    def arm_dof(self)->int:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'armRealDof')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("armRealDof parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get armRealDof: {e}")
-            return None
-    
-    def head_dof(self)->int:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'headRealDof')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("headRealDof parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get headRealDof: {e}")
-            return None
-    
-    def leg_dof(self)->int:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'legRealDof')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("legRealDof parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get legRealDof: {e}")
-            return None
-    
-    def end_effector_type(self)->str:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'end_effector_type')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("end_effector_type parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get end_effector_type: {e}")
-            return None
-    
-    def humanoid_description(self)->str:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'humanoid_description')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("humanoid_description parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get humanoid_description: {e}")
-            return None
-
-    def model_path(self)->str:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'modelPath')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("modelPath parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get modelPath: {e}")
-            return None
-
-    def kuavo_config(self)->str:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'kuavo_configuration')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("kuavo_configuration parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get kuavo_configuration: {e}")
-            return None
-
-    def initial_state(self)->str:
-        try:
-            param_service = roslibpy.Param(self.websocket.client, 'initial_state')
-            param = param_service.get()
-            if param is None:
-                SDKLogger.error("initial_state parameter not found")
-                return None
-            return param
-        except Exception as e:
-            SDKLogger.error(f"Failed to get initial_state: {e}")
-            return None
-
-# if GlobalConfig.use_websocket:
-#     kuavo_ros_param = RosParamWebsocket()
-# else:
-#     kuavo_ros_param = RosParameter()
+kuavo_ros_param = RosParameter()
 
 def joint_names()->dict:
     leg_link_names = [
@@ -201,11 +78,6 @@ def joint_names()->dict:
     head_link_names = [
         'zhead_1_link', 'zhead_2_link'
     ]
-    if GlobalConfig.use_websocket:
-        kuavo_ros_param = RosParamWebsocket()
-    else:
-        kuavo_ros_param = RosParameter()
-
     robot_desc = kuavo_ros_param.humanoid_description()
     if robot_desc is None:
         return None
@@ -263,11 +135,6 @@ kuavo_ros_info = None
 
 def end_frames_names()->dict:
     default = ["torso", "zarm_l7_link", "zarm_r7_link", "zarm_l4_link", "zarm_r4_link"]
-    if GlobalConfig.use_websocket:
-        kuavo_ros_param = RosParamWebsocket()
-    else:
-        kuavo_ros_param = RosParameter()
-
     kuavo_json = kuavo_ros_param.kuavo_config()
     if kuavo_json is None:
         return default
@@ -287,11 +154,6 @@ def make_robot_param()->dict:
     if kuavo_ros_info is not None:
         return kuavo_ros_info
     
-    if GlobalConfig.use_websocket:
-        kuavo_ros_param = RosParamWebsocket()
-    else:
-        kuavo_ros_param = RosParameter()
-
     kuavo_ros_info = {
         'robot_version': kuavo_ros_param.robot_version(),
         'arm_dof': kuavo_ros_param.arm_dof(),
@@ -312,4 +174,4 @@ def make_robot_param()->dict:
 
 if __name__ == "__main__":
     rospy.init_node("kuavo_ros_param_test")
-    print(make_robot_param())
+    print(make_robot_param())

kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -1,24 +1,17 @@
-import roslibpy
+import rospy
 import copy
 import time
 from typing import Tuple
-
-try:
-    import rospy
-    from std_msgs.msg import Float64
-    from nav_msgs.msg import Odometry
-    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 kuavo_humanoid_sdk.msg.ocs2_msgs.msg import mpc_observation
-except:
-    pass
-
+from std_msgs.msg import Float64
+from nav_msgs.msg import Odometry
+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 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 kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
 
 from collections import deque
 from typing import Tuple, Optional
@@ -57,7 +50,6 @@ class GaitManager:
             return "No_Gait"
         return self._gait_time_names[-1][1]
 
-
 class KuavoRobotStateCore:
     _instance = None
 
@@ -250,10 +242,10 @@ class KuavoRobotStateCore:
             self._gait_changed_callbacks.append(callback)
         
     """ ------------------------------- callback ------------------------------- """
-    def _terrain_height_callback(self, msg)->None:
+    def _terrain_height_callback(self, msg:Float64)->None:
         self._terrain_height = msg.data
 
-    def _sensors_data_raw_callback(self, msg)->None:
+    def _sensors_data_raw_callback(self, msg:sensorsData)->None:
         # update imu data
         self._imu_data = KuavoImuData(
             gyro = (msg.imu_data.gyro.x, msg.imu_data.gyro.y, msg.imu_data.gyro.z),
@@ -269,7 +261,7 @@ class KuavoRobotStateCore:
             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)->None:
+    def _odom_callback(self, msg:Odometry)->None:
         # update odom data
         self._odom_data = KuavoOdometry(
             position = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z),
@@ -278,7 +270,7 @@ class KuavoRobotStateCore:
             angular = (msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z)
         )
 
-    def _lejuclaw_state_callback(self, msg)->None:
+    def _lejuclaw_state_callback(self, msg:lejuClawState)->None:
         self._eef_state = (EndEffectorState(
             # left claw
             position = [msg.data.position[0]],
@@ -293,7 +285,7 @@ class KuavoRobotStateCore:
             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(
             # left hand
             position = list(msg.position[:len(msg.position)//2]),
@@ -308,7 +300,7 @@ class KuavoRobotStateCore:
             state=EndEffectorState.GraspingState.UNKNOWN
         ))
 
-    def _dexhand_touch_state_callback(self, msg)->None:
+    def _dexhand_touch_state_callback(self, msg:dexhandTouchState)->None:
         # Update touch state for both hands
         self._dexhand_touch_state = (
             KuavoDexHandTouchState(
@@ -331,7 +323,7 @@ class KuavoRobotStateCore:
             )   # Right hand touch state
         )
 
-    def _humanoid_mpc_gait_changed_callback(self, msg):
+    def _humanoid_mpc_gait_changed_callback(self, msg: gaitTimeName):
         """
         Callback function for gait change messages.
         Updates the current gait name when a gait change occurs.
@@ -339,7 +331,7 @@ class KuavoRobotStateCore:
         SDKLogger.debug(f"[State] Received gait change message: {msg.gait_name} at time {msg.start_time}")
         self._gait_manager.add(msg.start_time, msg.gait_name)
     
-    def _humanoid_mpc_observation_callback(self, msg) -> None:
+    def _humanoid_mpc_observation_callback(self, msg: mpc_observation) -> None:
         """
         Callback function for MPC observation messages.
         Updates the current MPC state and input data.
@@ -386,309 +378,3 @@ class KuavoRobotStateCore:
         except Exception as e:
             SDKLogger.error(f"Service call failed: {e}")
         return None
-
-class KuavoRobotStateCoreWebsocket:
-    _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'):
-            try:
-                self.websocket = WebSocketKuavoSDK()
-                if not self.websocket.client.is_connected:
-                    SDKLogger.error("Failed to connect to WebSocket server")
-                    raise ConnectionError("Failed to connect to WebSocket server")
-                
-                # Initialize subscribers
-                self._sub_sensors_data = roslibpy.Topic(self.websocket.client, '/sensors_data_raw', 'kuavo_msgs/sensorsData')
-                self._sub_odom = roslibpy.Topic(self.websocket.client, '/odom', 'nav_msgs/Odometry')
-                self._sub_terrain_height = roslibpy.Topic(self.websocket.client, '/humanoid/mpc/terrainHeight', 'std_msgs/Float64')
-                self._sub_gait_time_name = roslibpy.Topic(self.websocket.client, '/humanoid_mpc_gait_time_name', 'kuavo_msgs/gaitTimeName')
-                self._sub_mpc_observation = roslibpy.Topic(self.websocket.client, '/humanoid_mpc_observation', 'ocs2_msgs/mpc_observation')
-                
-                # service calls are time-consuming after subscription, place them before subscription
-                kuavo_info = make_robot_param()
-
-                # Subscribe to topics
-                self._sub_sensors_data.subscribe(self._sensors_data_raw_callback)
-                self._sub_odom.subscribe(self._odom_callback)
-                self._sub_terrain_height.subscribe(self._terrain_height_callback)
-                self._sub_gait_time_name.subscribe(self._humanoid_mpc_gait_changed_callback)
-                self._sub_mpc_observation.subscribe(self._humanoid_mpc_observation_callback)
-                
-                if kuavo_info is None:
-                    SDKLogger.error("Failed to get robot parameters")
-                    raise RuntimeError("Failed to get robot parameters")
-                
-                self._ee_type = kuavo_info['end_effector_type']
-                if self._ee_type == EndEffectorType.LEJUCLAW:
-                    self._sub_lejuclaw_state = roslibpy.Topic(self.websocket.client, '/leju_claw_state', 'kuavo_msgs/lejuClawState')
-                    self._sub_lejuclaw_state.subscribe(self._lejuclaw_state_callback)
-                elif self._ee_type == EndEffectorType.QIANGNAO:
-                    self._sub_dexhand_state = roslibpy.Topic(self.websocket.client, '/dexhand/state', 'sensor_msgs/JointState')
-                    self._sub_dexhand_state.subscribe(self._dexterous_hand_state_callback)
-                elif self._ee_type == EndEffectorType.QIANGNAO_TOUCH:
-                    self._sub_dexhand_state = roslibpy.Topic(self.websocket.client, '/dexhand/state', 'sensor_msgs/JointState')
-                    self._sub_dexhand_touch_state = roslibpy.Topic(self.websocket.client, '/dexhand/touch_state', 'kuavo_msgs/dexhandTouchState')
-                    self._sub_dexhand_state.subscribe(self._dexterous_hand_state_callback)
-                    self._sub_dexhand_touch_state.subscribe(self._dexhand_touch_state_callback)
-                    # Initialize touch state for both hands
-                    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))
-                        ),
-                        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))
-                        )
-                    )
-
-                """ data """
-                self._terrain_height = 0.0
-                self._imu_data = KuavoImuData(
-                    gyro = (0.0, 0.0, 0.0),
-                    acc = (0.0, 0.0, 0.0),
-                    free_acc = (0.0, 0.0, 0.0),
-                    quat = (0.0, 0.0, 0.0, 0.0)
-                )
-                self._joint_data = KuavoJointData(
-                    position = [0.0] * 28,
-                    velocity = [0.0] * 28,
-                    torque = [0.0] * 28,
-                    acceleration = [0.0] * 28
-                )
-                self._odom_data = KuavoOdometry(
-                    position = (0.0, 0.0, 0.0),
-                    orientation = (0.0, 0.0, 0.0, 0.0),
-                    linear = (0.0, 0.0, 0.0),
-                    angular = (0.0, 0.0, 0.0)
-                )
-                self._eef_state = (EndEffectorState(
-                    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] * 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
-
-                # Wait for first MPC observation data
-                self._mpc_observation_data = None
-                start_time = time.time()
-                while self._mpc_observation_data is None:
-                    if time.time() - start_time > 1.0:  # 1.0s timeout
-                        SDKLogger.warn("Timeout waiting for MPC observation data")
-                        break
-                    SDKLogger.debug("Waiting for first MPC observation data...")
-                    time.sleep(0.1)
-
-                if self._mpc_observation_data is not None:
-                    if self._gait_manager.is_empty:
-                        self._prev_gait_name = self.gait_name()
-                        SDKLogger.debug(f"[State] Adding initial gait state: {self._prev_gait_name} at time {self._mpc_observation_data['time']}")
-                        self._gait_manager.add(self._mpc_observation_data['time'], self._prev_gait_name)
-
-                # 获取当前手臂控制模式
-                self._arm_ctrl_mode = self._srv_get_arm_ctrl_mode()
-                self._initialized = True
-            except Exception as e:
-                SDKLogger.error(f"Failed to initialize KuavoRobotStateCoreWebsocket: {e}")
-                raise
-
-    @property
-    def com_height(self)->float:
-        return self._odom_data.position[2] - self._terrain_height
-    
-    @property
-    def imu_data(self)->KuavoImuData:
-        return self._imu_data
-    
-    @property
-    def joint_data(self)->KuavoJointData:
-        return self._joint_data
-    
-    @property
-    def odom_data(self)->KuavoOdometry:
-        return self._odom_data
-
-    @property
-    def arm_control_mode(self) -> KuavoArmCtrlMode:
-        mode = self._srv_get_arm_ctrl_mode()
-        if mode is not None:
-            self._arm_ctrl_mode = mode
-        return self._arm_ctrl_mode
-    
-    @property
-    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:
-        if self._mpc_observation_data is None:
-            return None
-        return self._mpc_observation_data.time
-    
-    @property
-    def current_gait_mode(self) -> int:
-        if self._mpc_observation_data is None:
-            return None
-        return self._mpc_observation_data.mode
-
-    def gait_name(self)->str:
-        return self._srv_get_current_gait_name()
-    
-    def is_gait(self, gait_name: str) -> bool:
-        return self._gait_manager.get_gait(self._mpc_observation_data['time']) == gait_name
-
-    def register_gait_changed_callback(self, callback):
-        if not hasattr(self, '_gait_changed_callbacks'):
-            self._gait_changed_callbacks = []
-        
-        from inspect import signature
-        sig = signature(callback)
-        if len(sig.parameters) != 2:
-            raise ValueError("Callback must accept 2 parameters: current_time (float), gait_name (str)")
-        if callback not in self._gait_changed_callbacks:
-            self._gait_changed_callbacks.append(callback)
-
-    def _terrain_height_callback(self, msg)->None:
-        self._terrain_height = msg['data']
-
-    def _sensors_data_raw_callback(self, msg)->None:
-        # update imu data
-        self._imu_data = KuavoImuData(
-            gyro = (msg['imu_data']['gyro']['x'], msg['imu_data']['gyro']['y'], msg['imu_data']['gyro']['z']),
-            acc = (msg['imu_data']['acc']['x'], msg['imu_data']['acc']['y'], msg['imu_data']['acc']['z']),
-            free_acc = (msg['imu_data']['free_acc']['x'], msg['imu_data']['free_acc']['y'], msg['imu_data']['free_acc']['z']),
-            quat = (msg['imu_data']['quat']['x'], msg['imu_data']['quat']['y'], msg['imu_data']['quat']['z'], msg['imu_data']['quat']['w'])
-        )
-        # update joint data
-        self._joint_data = KuavoJointData(
-            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'].get('joint_current', msg['joint_data']['joint_torque']))
-        )
-
-    def _odom_callback(self, msg)->None:
-        # update odom data
-        self._odom_data = KuavoOdometry(
-            position = (msg['pose']['pose']['position']['x'], msg['pose']['pose']['position']['y'], msg['pose']['pose']['position']['z']),
-            orientation = (msg['pose']['pose']['orientation']['x'], msg['pose']['pose']['orientation']['y'], msg['pose']['pose']['orientation']['z'], msg['pose']['pose']['orientation']['w']),
-            linear = (msg['twist']['twist']['linear']['x'], msg['twist']['twist']['linear']['y'], msg['twist']['twist']['linear']['z']),
-            angular = (msg['twist']['twist']['angular']['x'], msg['twist']['twist']['angular']['y'], msg['twist']['twist']['angular']['z'])
-        )
-
-    def _lejuclaw_state_callback(self, msg)->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])
-        ), EndEffectorState(
-            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:
-        self._eef_state = (EndEffectorState(
-            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 = 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)->None:
-        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'])
-            ),
-            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'])
-            )
-        )
-
-    def _humanoid_mpc_gait_changed_callback(self, msg):
-        SDKLogger.debug(f"[State] Received gait change message: {msg['gait_name']} at time {msg['start_time']}")
-        self._gait_manager.add(msg['start_time'], msg['gait_name'])
-    
-    def _humanoid_mpc_observation_callback(self, msg) -> None:
-        try:
-            SDKLogger.debug(f"[State] Received MPC observation message: {msg}")
-            self._mpc_observation_data = msg
-            if hasattr(self, '_initialized'): 
-                curr_time = self._mpc_observation_data['time']
-                current_gait = self._gait_manager.get_gait(curr_time)
-                if self._prev_gait_name != current_gait:
-                    SDKLogger.debug(f"[State] Gait changed to: {current_gait} at time {curr_time}")
-                    self._prev_gait_name = current_gait
-                    if hasattr(self, '_gait_changed_callbacks') and self._gait_changed_callbacks is not None:
-                        for callback in self._gait_changed_callbacks:
-                            callback(curr_time, current_gait)
-        except Exception as e:
-            SDKLogger.error(f"Error processing MPC observation: {e}")
-
-    def _srv_get_arm_ctrl_mode(self)-> KuavoArmCtrlMode:
-        try:
-            service = roslibpy.Service(self.websocket.client, '/humanoid_get_arm_ctrl_mode', 'kuavo_msgs/changeArmCtrlMode')
-            request = {}
-            response = service.call(request)
-            return KuavoArmCtrlMode(response.get('mode', 0))
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        return None
-    
-    def _srv_get_current_gait_name(self)->str:
-        try:
-            service = roslibpy.Service(self.websocket.client, '/humanoid_get_current_gait_name', 'kuavo_msgs/getCurrentGaitName')
-            request = {}
-            response = service.call(request)
-            if response.get('success', False):
-                return response['gait_name']
-            return None
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        return None