kuavo-humanoid-sdk 1.1.5__py3-none-any.whl → 1.1.6__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -1,26 +1,18 @@
-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 kuavo_humanoid_sdk.interfaces.data_types import (KuavoImuData, KuavoJointData, KuavoOdometry, KuavoManipulationMpcFrame, 
-                                                      KuavoArmCtrlMode, EndEffectorState, KuavoDexHandTouchState,
-                                                      KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow)
+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 kuavo_humanoid_sdk.msg.kuavo_msgs.srv import (changeArmCtrlMode, changeArmCtrlModeRequest,setMmCtrlFrame, setMmCtrlFrameRequest,
-                                                   changeTorsoCtrlMode, changeTorsoCtrlModeRequest)
+
 from collections import deque
 from typing import Tuple, Optional
 
@@ -58,7 +50,6 @@ class GaitManager:
             return "No_Gait"
         return self._gait_time_names[-1][1]
 
-
 class KuavoRobotStateCore:
     _instance = None
 
@@ -158,11 +149,6 @@ class KuavoRobotStateCore:
             self._arm_ctrl_mode = self._srv_get_arm_ctrl_mode()
             self._initialized = True
 
-            # 获取manipulation mpc 相关参数
-            self._manipulation_mpc_frame = self._srv_get_manipulation_mpc_frame()
-            self._manipulation_mpc_ctrl_mode = self._srv_get_manipulation_mpc_ctrl_mode()
-            self._manipulation_mpc_control_flow = self._srv_get_manipulation_mpc_control_flow()
-
     @property
     def com_height(self)->float:
         # odom.position.z - terrain_height = com_height
@@ -187,27 +173,6 @@ class KuavoRobotStateCore:
             self._arm_ctrl_mode = mode
         return self._arm_ctrl_mode
     
-    @property
-    def manipulation_mpc_ctrl_mode(self)->KuavoManipulationMpcCtrlMode:
-        mode = self._srv_get_manipulation_mpc_ctrl_mode()
-        if mode is not None:
-            self._manipulation_mpc_ctrl_mode = mode
-        return self._manipulation_mpc_ctrl_mode
-    
-    @property
-    def manipulation_mpc_frame(self)->KuavoManipulationMpcFrame:
-        frame = self._srv_get_manipulation_mpc_frame()
-        if frame is not None:
-            self._manipulation_mpc_frame = frame
-        return self._manipulation_mpc_frame
-    
-    @property
-    def manipulation_mpc_control_flow(self)->KuavoManipulationMpcControlFlow:
-        flow = self._srv_get_manipulation_mpc_control_flow()
-        if flow is not None:
-            self._manipulation_mpc_control_flow = flow
-        return self._manipulation_mpc_control_flow
-    
     @property
     def eef_state(self)->Tuple[EndEffectorState, EndEffectorState]:
         return self._eef_state
@@ -277,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),
@@ -296,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),
@@ -305,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]],
@@ -320,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]),
@@ -335,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(
@@ -358,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.
@@ -366,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.
@@ -413,505 +378,3 @@ class KuavoRobotStateCore:
         except Exception as e:
             SDKLogger.error(f"Service call failed: {e}")
         return None
-
-    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
-        try:
-            service_name = '/mobile_manipulator_get_mpc_control_mode'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
-            
-            req = changeTorsoCtrlModeRequest()
-            
-            resp = get_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
-                return KuavoManipulationMpcCtrlMode.ERROR
-            return KuavoManipulationMpcCtrlMode(resp.mode)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
-        return KuavoManipulationMpcCtrlMode.ERROR
-
-    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
-        try:
-            service_name = '/get_mm_ctrl_frame'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
-            
-            req = setMmCtrlFrameRequest()
-            
-            resp = get_frame_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
-                return KuavoManipulationMpcFrame.ERROR
-            return KuavoManipulationMpcFrame(resp.currentFrame)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
-        return KuavoManipulationMpcFrame.ERROR
-
-    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
-        try:
-            service_name = '/get_mm_wbc_arm_trajectory_control'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
-            
-            req = changeArmCtrlModeRequest()
-            
-            resp = get_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
-                return KuavoManipulationMpcControlFlow.Error
-            return KuavoManipulationMpcControlFlow(resp.mode)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
-        return KuavoManipulationMpcControlFlow.Error
-
-    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
-        try:
-            service_name = '/mobile_manipulator_get_mpc_control_mode'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
-            
-            req = changeTorsoCtrlModeRequest()
-            
-            resp = get_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
-                return KuavoManipulationMpcCtrlMode.ERROR
-            return KuavoManipulationMpcCtrlMode(resp.mode)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
-        return KuavoManipulationMpcCtrlMode.ERROR
-
-    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
-        try:
-            service_name = '/get_mm_ctrl_frame'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
-            
-            req = setMmCtrlFrameRequest()
-            
-            resp = get_frame_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
-                return KuavoManipulationMpcFrame.ERROR
-            return KuavoManipulationMpcFrame(resp.currentFrame)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
-        return KuavoManipulationMpcFrame.ERROR
-
-    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
-        try:
-            service_name = '/get_mm_wbc_arm_trajectory_control'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
-            
-            req = changeArmCtrlModeRequest()
-            
-            resp = get_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
-                return KuavoManipulationMpcControlFlow.Error
-            return KuavoManipulationMpcControlFlow(resp.mode)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
-        return KuavoManipulationMpcControlFlow.Error
-
-    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
-        try:
-            service_name = '/mobile_manipulator_get_mpc_control_mode'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
-            
-            req = changeTorsoCtrlModeRequest()
-            
-            resp = get_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
-                return KuavoManipulationMpcCtrlMode.ERROR
-            return KuavoManipulationMpcCtrlMode(resp.mode)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
-        return KuavoManipulationMpcCtrlMode.ERROR
-
-    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
-        try:
-            service_name = '/get_mm_ctrl_frame'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
-            
-            req = setMmCtrlFrameRequest()
-            
-            resp = get_frame_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
-                return KuavoManipulationMpcFrame.ERROR
-            return KuavoManipulationMpcFrame(resp.currentFrame)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
-        return KuavoManipulationMpcFrame.ERROR
-
-    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
-        try:
-            service_name = '/get_mm_wbc_arm_trajectory_control'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
-            
-            req = changeArmCtrlModeRequest()
-            
-            resp = get_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
-                return KuavoManipulationMpcControlFlow.Error
-            return KuavoManipulationMpcControlFlow(resp.mode)
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call to {service_name} failed: {e}")
-        except rospy.ROSException as e: # For timeout from wait_for_service
-            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
-        return KuavoManipulationMpcControlFlow.Error
-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
-
-
-if __name__ == "__main__":
-    state = KuavoRobotStateCore()
-    print(state.manipulation_mpc_frame)
-    print(state.manipulation_mpc_control_flow)
-    print(state.manipulation_mpc_ctrl_mode)
-    print(state.arm_control_mode)