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

kuavo_humanoid_sdk/kuavo/core/ros/control.py

@@ -1,34 +1,19 @@
-from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 import os
-import roslibpy
+import rospy
 import numpy as np
 from typing import Tuple
 from kuavo_humanoid_sdk.common.logger import SDKLogger
-from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
-from kuavo_humanoid_sdk.interfaces.data_types import (KuavoArmCtrlMode, KuavoIKParams, KuavoPose, 
-                                                      KuavoManipulationMpcControlFlow, KuavoManipulationMpcCtrlMode
-                                                      ,KuavoManipulationMpcFrame)
+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
-
-try:
-    import rospy
-    from geometry_msgs.msg import Twist
-    from sensor_msgs.msg import JointState, Joy
-    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,
-                            changeTorsoCtrlMode, changeTorsoCtrlModeRequest, setMmCtrlFrame, setMmCtrlFrameRequest,
-                            setTagId, setTagIdRequest)
-    from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import twoArmHandPoseCmd, ikSolveParam, armHandPose
-    from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import twoArmHandPoseCmdSrv, fkSrv
-    from std_srvs.srv import SetBool, SetBoolRequest
-except:
-    pass
-
-
-
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import JointState, Joy
+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 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 = EndEffectorType.QIANGNAO):
@@ -190,210 +175,11 @@ class ControlEndEffector:
             SDKLogger.error(f"Service `control_robot_leju_claw` call failed: {e}")
         return False
 
-class ControlEndEffectorWebsocket:
-    def __init__(self, eef_type: str = EndEffectorType.QIANGNAO):
-        self._eef_type = eef_type
-        self._pubs = []
-        websocket = WebSocketKuavoSDK()
-        if self._eef_type == EndEffectorType.QIANGNAO:
-            self._pub_ctrl_robot_hand = roslibpy.Topic(websocket.client, '/control_robot_hand_position', 'kuavo_msgs/robotHandPosition')                
-            # publisher, name, require
-            self._pubs.append((self._pub_ctrl_robot_hand, True))
-        elif self._eef_type == EndEffectorType.QIANGNAO_TOUCH:
-            self._pub_ctrl_robot_hand = roslibpy.Topic(websocket.client, '/control_robot_hand_position', 'kuavo_msgs/robotHandPosition')                
-            self._pub_dexhand_command = roslibpy.Topic(websocket.client, '/dexhand/command', 'kuavo_msgs/dexhandCommand')
-            self._pub_dexhand_right_command = roslibpy.Topic(websocket.client, '/dexhand/right/command', 'kuavo_msgs/dexhandCommand')
-            self._pub_dexhand_left_command = roslibpy.Topic(websocket.client, '/dexhand/left/command', 'kuavo_msgs/dexhandCommand')
-            # 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:
-        return True
-
-    def pub_control_robot_dexhand(self, left_position:list, right_position:list)->bool:
-        if not self._eef_type.startswith(EndEffectorType.QIANGNAO): # qiangnao, qiangnao_touch
-            SDKLogger.warning(f"{self._eef_type} not support control dexhand")
-            return False
-        try:
-            hand_pose_msg = {
-                "left_hand_position": bytes(left_position),
-                "right_hand_position": bytes(right_position)
-            }
-            self._pub_ctrl_robot_hand.publish(roslibpy.Message(hand_pose_msg))
-            SDKLogger.debug(f"publish robot dexhand: {left_position}, {right_position}")
-            return True
-        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 = {
-                "data": [int(d) for d in data],  # Convert data to integers
-                "control_mode": ctrl_mode
-            }
-            if hand_side == 0:
-                self._pub_dexhand_left_command.publish(roslibpy.Message(msg))
-            elif hand_side == 1:
-                self._pub_dexhand_right_command.publish(roslibpy.Message(msg))
-            else:
-                self._pub_dexhand_command.publish(roslibpy.Message(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 not self._eef_type.startswith(EndEffectorType.QIANGNAO): # qiangnao, qiangnao_touch
-            SDKLogger.warning(f"{self._eef_type} not support control dexhand")
-            return False
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, 'gesture/execute', 'kuavo_msgs/gestureExecute')
-            request = {
-                "gestures": [
-                    {
-                        "gesture_name": gs["gesture_name"],
-                        "hand_side": gs["hand_side"]
-                    } for gs in gestures
-                ]
-            }
-            response = service.call(request)
-            if not response.get('success', False):
-                SDKLogger.error(f"Failed to execute gesture '{gestures}': {response.get('message', '')}")
-            return response.get('success', False)
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-            return False
-
-    def srv_get_gesture_names(self)->list:
-        if not self._eef_type.startswith(EndEffectorType.QIANGNAO): # qiangnao, qiangnao_touch
-            SDKLogger.warning(f"{self._eef_type} not support control dexhand")
-            return []
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, 'gesture/list', 'kuavo_msgs/gestureList')
-            request = {}
-            response = service.call(request)
-            gestures = []
-            for gesture_info in response.get('gesture_infos', []):
-                gestures.append(gesture_info['gesture_name'])
-                for alias in gesture_info.get('alias', []):
-                    gestures.append(alias)
-            return list(set(gestures))
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-            return []
-
-    def srv_control_leju_claw(self, postions:list, velocities:list, torques:list) ->bool:
-        if self._eef_type != 'lejuclaw':
-            SDKLogger.warning(f"{self._eef_type} not support control lejuclaw.")
-            return False
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, 'control_robot_leju_claw', 'kuavo_msgs/controlLejuClaw')
-            request = {
-                "data": {
-                    "position": postions,
-                    "velocity": velocities,
-                    "effort": torques
-                }
-            }
-            response = service.call(request)
-            if not response.get('result', False):
-                SDKLogger.error(f"Failed to control leju claw: {response.get('message', '')}")
-            return response.get('result', False)
-        except Exception as e:
-            SDKLogger.error(f"Service `control_robot_leju_claw` call failed: {e}")
-            return False
-
-class ControlRobotArmWebsocket:
-    def __init__(self):
-        websocket = WebSocketKuavoSDK()
-        self._pub_ctrl_arm_traj = roslibpy.Topic(websocket.client, '/kuavo_arm_traj', 'sensor_msgs/JointState')
-        self._pub_ctrl_arm_traj.advertise()
-        self._pub_ctrl_arm_target_poses = roslibpy.Topic(websocket.client, '/kuavo_arm_target_poses', 'kuavo_msgs/armTargetPoses')
-        self._pub_ctrl_arm_target_poses.advertise()
-    def connect(self, timeout:float=1.0)-> bool:
-        return True
-
-    def pub_control_robot_arm_traj(self, joint_q: list)->bool:
-        try:
-            msg = {
-                "name": ["arm_joint_" + str(i) for i in range(0, 14)],
-                "position": [float(180.0 / np.pi * q) for q in joint_q]  # convert to degree
-            }
-            self._pub_ctrl_arm_traj.publish(roslibpy.Message(msg))
-            return True
-        except Exception as e:
-            SDKLogger.error(f"publish robot arm traj: {e}")
-        return False
-        
-    def pub_arm_target_poses(self, times:list, joint_q:list):
-        try:
-            msg_values = []
-            for i in range(len(joint_q)):
-                degs = [float(q) for q in joint_q[i]]
-                msg_values.extend(degs)
-            msg = {
-                "times": [float(q) for q in times],
-                "values": msg_values
-            }
-            self._pub_ctrl_arm_target_poses.publish(roslibpy.Message(msg))
-            return True
-        except Exception as e:
-            SDKLogger.error(f"publish arm target poses: {e}")
-            
-        return False
-
-    def srv_change_arm_ctrl_mode(self, mode: KuavoArmCtrlMode)->bool:
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, '/arm_traj_change_mode', 'kuavo_msgs/changeArmCtrlMode')
-            request = {
-                "control_mode": mode.value
-            }
-            response = service.call(request)
-            return response.get('result', False)
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        return False
-    
-    def srv_get_arm_ctrl_mode(self)-> KuavoArmCtrlMode:
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, '/humanoid_get_arm_ctrl_mode', 'kuavo_msgs/changeArmCtrlMode')
-            request = {}
-            response = service.call(request)
-            return KuavoArmCtrlMode(response.get('control_mode', 0))
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        return None
-
+""" Control Robot Arm"""
 class ControlRobotArm:
     def __init__(self):
         self._pub_ctrl_arm_traj = rospy.Publisher('/kuavo_arm_traj', JointState, queue_size=10)
         self._pub_ctrl_arm_target_poses = rospy.Publisher('/kuavo_arm_target_poses', armTargetPoses, queue_size=10)
-        self._pub_ctrl_hand_pose_cmd = rospy.Publisher('/mm/two_arm_hand_pose_cmd', twoArmHandPoseCmd, queue_size=10)
 
     def connect(self, timeout:float=1.0)-> bool:
         start_time = rospy.Time.now()
@@ -436,153 +222,6 @@ class ControlRobotArm:
         except Exception as e:
             SDKLogger.error(f"publish arm target poses: {e}")
         return False
-    def pub_end_effector_pose_cmd(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:
-        try:
-            msg = twoArmHandPoseCmd()
-            left_pose_msg = armHandPose()
-            left_pose_msg.pos_xyz = left_pose.position
-            left_pose_msg.quat_xyzw = left_pose.orientation
-            right_pose_msg = armHandPose()
-            right_pose_msg.pos_xyz = right_pose.position
-            right_pose_msg.quat_xyzw = right_pose.orientation
-            msg.hand_poses.left_pose = left_pose_msg
-            msg.hand_poses.right_pose = right_pose_msg
-            if frame.value not in [0, 1, 2, 3, 4]:
-                SDKLogger.error(f"Invalid frame: {frame}")
-                return False
-            msg.frame = frame.value
-            self._pub_ctrl_hand_pose_cmd.publish(msg)
-            return True
-        except Exception as e:
-            SDKLogger.error(f"publish arm target poses: {e}")
-        return False
-
-    def srv_change_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame)->bool:
-        try:
-            service_name = '/set_mm_ctrl_frame'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            set_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
-            
-            req = setMmCtrlFrameRequest()
-            req.frame = frame.value
-            
-            resp = set_frame_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to change manipulation mpc frame to {frame}: {resp.message}")
-            return resp.result
-        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 change manipulation mpc frame: {e}")
-        return False
-    
-    def srv_change_manipulation_mpc_ctrl_mode(self, ctrl_mode: KuavoManipulationMpcCtrlMode)->bool:
-        try:
-            service_name = '/mobile_manipulator_mpc_control'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            set_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
-            
-            req = changeTorsoCtrlModeRequest()
-            req.control_mode = ctrl_mode.value
-            
-            resp = set_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to change manipulation mpc control mode to {ctrl_mode}: {resp.message}")
-            return resp.result
-        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 change manipulation mpc control mode: {e}")
-        return False
-
-    def srv_change_manipulation_mpc_control_flow(self, ctrl_flow: KuavoManipulationMpcControlFlow)-> bool:
-        try:
-            service_name = '/enable_mm_wbc_arm_trajectory_control'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            set_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
-
-            req = changeArmCtrlModeRequest()
-            req.control_mode = ctrl_flow.value
-
-            resp = set_mode_srv(req)
-            if not resp.result:
-                SDKLogger.error(f"Failed to change manipulation mpc wbc arm trajectory control to {ctrl_flow}: {resp.message}")
-            return resp.result
-        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 change manipulation mpc control flow: {e}")
-        return False
-
-    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_change_arm_ctrl_mode(self, mode: KuavoArmCtrlMode)->bool:
         try:
@@ -615,7 +254,7 @@ class ControlRobotArm:
 class ControlRobotHead:
     def __init__(self):
         self._pub_ctrl_robot_head = rospy.Publisher('/robot_head_motion_data', robotHeadMotionData, queue_size=10)
-    
+
     def connect(self, timeout:float=1.0)->bool:
         start_time = rospy.Time.now()
         publishers = [
@@ -638,188 +277,6 @@ class ControlRobotHead:
             SDKLogger.error(f"[Error] publish robot head: {e}")
             return False
 
-    def srv_enable_head_tracking(self, target_id: int)->bool:
-        """Enable the head tracking for a specific tag ID.
-        
-        Args:
-            target_id: The ID of the tag to track
-            
-        Returns:
-            bool: True if successful, False otherwise
-        """
-        try:
-            # 首先设置追踪目标ID
-            service_name = '/set_target_tag_id'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            set_tag_id_srv = rospy.ServiceProxy(service_name, setTagId)
-            
-            req = setTagIdRequest()
-            req.tag_id = target_id
-            
-            resp = set_tag_id_srv(req)
-            if not resp.success:
-                SDKLogger.error(f"Failed to set target tag ID: {resp.message}")
-                return False
-                
-            SDKLogger.info(f"Successfully set target tag ID to {target_id}: {resp.message}")
-            
-            # 然后启动连续追踪
-            service_name = '/continuous_track'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            continuous_track_srv = rospy.ServiceProxy(service_name, SetBool)
-            
-            req = SetBoolRequest()
-            req.data = True
-            
-            resp = continuous_track_srv(req)
-            if not resp.success:
-                SDKLogger.error(f"Failed to start continuous tracking: {resp.message}")
-                return False
-                
-            SDKLogger.info(f"Successfully started continuous tracking: {resp.message}")
-            return True
-            
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        except rospy.ROSException as e:
-            SDKLogger.error(f"Failed to connect to service: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to enable head tracking: {e}")
-            
-        return False
-        
-    def srv_disable_head_tracking(self)->bool:
-        """Disable the head tracking.
-        
-        Returns:
-            bool: True if successful, False otherwise
-        """
-        try:
-            service_name = '/continuous_track'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            continuous_track_srv = rospy.ServiceProxy(service_name, SetBool)
-            
-            req = SetBoolRequest()
-            req.data = False
-            
-            resp = continuous_track_srv(req)
-            if not resp.success:
-                SDKLogger.error(f"Failed to stop continuous tracking: {resp.message}")
-                return False
-                
-            SDKLogger.info(f"Successfully stopped continuous tracking: {resp.message}")
-            return True
-            
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        except rospy.ROSException as e:
-            SDKLogger.error(f"Failed to connect to service: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to disable head tracking: {e}")
-            
-        return False
-
-    def srv_enable_head_tracking(self, target_id: int)->bool:
-        """Enable the head tracking for a specific tag ID.
-        
-        Args:
-            target_id: The ID of the tag to track
-            
-        Returns:
-            bool: True if successful, False otherwise
-        """
-        try:
-            # 首先设置追踪目标ID
-            service_name = '/set_target_tag_id'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            set_tag_id_srv = rospy.ServiceProxy(service_name, setTagId)
-            
-            req = setTagIdRequest()
-            req.tag_id = target_id
-            
-            resp = set_tag_id_srv(req)
-            if not resp.success:
-                SDKLogger.error(f"Failed to set target tag ID: {resp.message}")
-                return False
-                
-            SDKLogger.info(f"Successfully set target tag ID to {target_id}: {resp.message}")
-            
-            # 然后启动连续追踪
-            service_name = '/continuous_track'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            continuous_track_srv = rospy.ServiceProxy(service_name, SetBool)
-            
-            req = SetBoolRequest()
-            req.data = True
-            
-            resp = continuous_track_srv(req)
-            if not resp.success:
-                SDKLogger.error(f"Failed to start continuous tracking: {resp.message}")
-                return False
-                
-            SDKLogger.info(f"Successfully started continuous tracking: {resp.message}")
-            return True
-            
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        except rospy.ROSException as e:
-            SDKLogger.error(f"Failed to connect to service: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to enable head tracking: {e}")
-            
-        return False
-        
-    def srv_disable_head_tracking(self)->bool:
-        """Disable the head tracking.
-        
-        Returns:
-            bool: True if successful, False otherwise
-        """
-        try:
-            service_name = '/continuous_track'
-            rospy.wait_for_service(service_name, timeout=2.0)
-            continuous_track_srv = rospy.ServiceProxy(service_name, SetBool)
-            
-            req = SetBoolRequest()
-            req.data = False
-            
-            resp = continuous_track_srv(req)
-            if not resp.success:
-                SDKLogger.error(f"Failed to stop continuous tracking: {resp.message}")
-                return False
-                
-            SDKLogger.info(f"Successfully stopped continuous tracking: {resp.message}")
-            return True
-            
-        except rospy.ServiceException as e:
-            SDKLogger.error(f"Service call failed: {e}")
-        except rospy.ROSException as e:
-            SDKLogger.error(f"Failed to connect to service: {e}")
-        except Exception as e:
-            SDKLogger.error(f"Failed to disable head tracking: {e}")
-            
-        return False
-
-""" Control Robot Head """
-class ControlRobotHeadWebsocket:
-    def __init__(self):
-        websocket = WebSocketKuavoSDK()
-        self._pub_ctrl_robot_head = roslibpy.Topic(websocket.client, '/robot_head_motion_data', 'kuavo_msgs/robotHeadMotionData')
-
-    def connect(self, timeout:float=1.0)->bool:
-        return True
-
-    def pub_control_robot_head(self, yaw:float, pitch:float)->bool:
-        try:
-            msg = {
-                "joint_data": [float(yaw), float(pitch)]
-            }
-            self._pub_ctrl_robot_head.publish(roslibpy.Message(msg))
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish robot head: {e}")
-            return False
-
 """ Control Robot Motion """
 
 # JoyButton constants
@@ -842,12 +299,10 @@ AXIS_RIGHT_RT = 5  # 1 -> (-1)
 AXIS_LEFT_RIGHT_TRIGGER = 6
 AXIS_FORWARD_BACK_TRIGGER = 7
 
-
 class ControlRobotMotion:
     def __init__(self):
         self._pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
         self._pub_cmd_pose = rospy.Publisher('/cmd_pose', Twist, queue_size=10)
-        self._pub_cmd_pose_world = rospy.Publisher('/cmd_pose_world', Twist, queue_size=10)
         self._pub_joy = rospy.Publisher('/joy', Joy, queue_size=10)
         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)
@@ -859,8 +314,7 @@ class ControlRobotMotion:
             (self._pub_cmd_vel, "cmd_vel publisher"),
             (self._pub_cmd_pose, "cmd_pose publisher"),
             (self._pub_step_ctrl, "step_ctrl publisher"),
-            (self._pub_switch_gait, "switch_gait publisher"),
-            (self._pub_cmd_pose_world, "cmd_pose_world publisher"),
+            (self._pub_switch_gait, "switch_gait publisher")
         ]
         
         success = True
@@ -885,22 +339,14 @@ class ControlRobotMotion:
             SDKLogger.error(f"[Error] publish cmd vel: {e}")
             return False
 
-    def pub_cmd_pose(self, twist)->bool:
+    def pub_cmd_pose(self, twist:Twist)->bool:
         try:
             self._pub_cmd_pose.publish(twist)
             return True
         except Exception as e:
             SDKLogger.error(f"[Error] publish cmd pose: {e}")
             return False    
-
-    def pub_cmd_pose_world(self, twist:Twist)->bool:
-        try:
-            self._pub_cmd_pose_world.publish(twist)
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish cmd pose world: {e}")
-            return False
-
+    
     def _create_joy_msg(self)->Joy:
         joy_msg = Joy()
         joy_msg.axes = [0.0] * 8  # Initialize 8 axes
@@ -947,114 +393,17 @@ class ControlRobotMotion:
         # return self._pub_joy_command(BUTTON_B, "trot")
         return self._pub_switch_gait_by_name("walk")
     
-    def pub_step_ctrl(self, msg)->bool:
+    def pub_step_ctrl(self, msg:footPoseTargetTrajectories)->bool:
         try:
             self._pub_step_ctrl.publish(msg)
             return True
         except Exception as e:
             SDKLogger.error(f"[Error] publish step ctrl: {e}")
             return False
-   
-
-class ControlRobotMotionWebsocket:
-    def __init__(self):
-        websocket = WebSocketKuavoSDK()
-        self._pub_cmd_vel = roslibpy.Topic(websocket.client, '/cmd_vel', 'geometry_msgs/Twist')
-        self._pub_cmd_pose = roslibpy.Topic(websocket.client, '/cmd_pose', 'geometry_msgs/Twist')
-        self._pub_joy = roslibpy.Topic(websocket.client, '/joy', 'sensor_msgs/Joy')
-        self._pub_switch_gait = roslibpy.Topic(websocket.client, '/humanoid_switch_gait_by_name', 'kuavo_msgs/switchGaitByName')
-        self._pub_step_ctrl = roslibpy.Topic(websocket.client, '/humanoid_mpc_foot_pose_target_trajectories', 'kuavo_msgs/footPoseTargetTrajectories')
-
-    def connect(self, timeout:float=2.0)-> bool:
-        return True
-
-    def pub_cmd_vel(self, linear_x: float, linear_y: float, angular_z: float)->bool:
-        try:
-            msg = {
-                "linear": {"x": float(linear_x), "y": float(linear_y), "z": 0.0},
-                "angular": {"x": 0.0, "y": 0.0, "z": float(angular_z)}
-            }
-            self._pub_cmd_vel.publish(roslibpy.Message(msg))
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish cmd vel: {e}")
-            return False
-
-    def pub_cmd_pose(self, height:float, pitch:float)->bool:
-        # com_msg = Twist()
-        # com_msg.linear.z = height
-        # com_msg.angular.y = pitch
-        try:
-            msg = {
-                "linear": {"x": 0, "y": 0, "z": float(height)},
-                "angular": {"x": 0, "y": float(pitch), "z": 0}
-            }
-            self._pub_cmd_pose.publish(roslibpy.Message(msg))
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish cmd pose: {e}")
-            return False
-
-    def _pub_joy_command(self, button_index: int, command_name: str) -> bool:
-        try:
-            msg = {
-                "axes": [0.0] * 8,
-                "buttons": [0] * 16
-            }
-            msg["buttons"][button_index] = 1
-            self._pub_joy.publish(roslibpy.Message(msg))
-            SDKLogger.debug(f"Published {command_name} command")
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish {command_name}: {e}")
-            return False
-
-    def _pub_switch_gait_by_name(self, gait_name: str) -> bool:
-        try:
-            msg = {
-                "gait_name": gait_name
-            }
-            self._pub_switch_gait.publish(roslibpy.Message(msg))
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish switch gait {gait_name}: {e}")
-            return False
-
-    def pub_walk_command(self) -> bool:
-        return self._pub_switch_gait_by_name("walk")
-
-    def pub_stance_command(self) -> bool:
-        try:
-            self.pub_cmd_vel(linear_x=0.0, linear_y=0.0, angular_z=0.0)
-            return self._pub_switch_gait_by_name("stance")
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish stance: {e}")
-            return False
-
-    def pub_trot_command(self) -> bool:
-        return self._pub_switch_gait_by_name("walk")
-    
-    def pub_step_ctrl(self, msg)->bool:
-        try:
-            websocket_msg = {
-                "timeTrajectory": msg.timeTrajectory,
-                "footIndexTrajectory": msg.footIndexTrajectory,
-                "footPoseTrajectory": [
-                    {
-                        "footPose": list(fp.footPose),
-                        "torsoPose": list(fp.torsoPose)
-                    } for fp in msg.footPoseTrajectory
-                ]
-            }
-            SDKLogger.debug(f"websocket_msg {websocket_msg}")
-            self._pub_step_ctrl.publish(roslibpy.Message(websocket_msg))
-            SDKLogger.debug(f"after publish")
-            return True
-        except Exception as e:
-            SDKLogger.error(f"[Error] publish step ctrl: {e}")
-            return False
-
-
+        
+"""
+Kuavo Robot Arm IK&FK
+"""
 class KuavoRobotArmIKFK:
     def __init__(self):
         pass
@@ -1099,7 +448,7 @@ class KuavoRobotArmIKFK:
     def arm_fk(self, q: list) -> Tuple[KuavoPose, KuavoPose]:
         return self._srv_arm_fk(q)
     
-    def _srv_arm_ik(self, eef_pose_msg)->list:
+    def _srv_arm_ik(self, eef_pose_msg:twoArmHandPoseCmd)->list:
         try:
             rospy.wait_for_service('/ik/two_arm_hand_pose_cmd_srv',timeout=1.0)
             ik_srv = rospy.ServiceProxy('/ik/two_arm_hand_pose_cmd_srv', twoArmHandPoseCmdSrv)
@@ -1131,88 +480,6 @@ class KuavoRobotArmIKFK:
         except Exception as e:
             print(f"Failed to call ik/fk_srv: {e}")
             return None
-
-class KuavoRobotArmIKFKWebsocket:
-    def __init__(self):
-        pass
-
-    def arm_ik(self, 
-               left_pose: KuavoPose, 
-               right_pose: KuavoPose, 
-               left_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
-               right_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
-               arm_q0: list = None,
-               params: KuavoIKParams=None) -> list:
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, '/ik/two_arm_hand_pose_cmd_srv', 'motion_capture_ik/twoArmHandPoseCmdSrv')
-            
-            request = {
-                "twoArmHandPoseCmdRequest": {
-                    "hand_poses": {
-                        "header": {
-                            "seq": 0,
-                            "stamp": {
-                                "secs": 0,
-                                "nsecs": 0
-                            },
-                            "frame_id": ""
-                        },
-                        "left_pose": {
-                            "pos_xyz": left_pose.position,
-                            "quat_xyzw": left_pose.orientation,
-                            "elbow_pos_xyz": left_elbow_pos_xyz,
-                        },
-                        "right_pose": {
-                            "pos_xyz": right_pose.position,
-                            "quat_xyzw": right_pose.orientation,
-                            "elbow_pos_xyz": right_elbow_pos_xyz,
-                        }
-                    },
-                    "use_custom_ik_param": params is not None,
-                    "joint_angles_as_q0": arm_q0 is not None,
-                    "ik_param": {
-                        "major_optimality_tol": params.major_optimality_tol if params else 0.0,
-                        "major_feasibility_tol": params.major_feasibility_tol if params else 0.0,
-                        "minor_feasibility_tol": params.minor_feasibility_tol if params else 0.0,
-                        "major_iterations_limit": params.major_iterations_limit if params else 0,
-                        "oritation_constraint_tol": params.oritation_constraint_tol if params else 0.0,
-                        "pos_constraint_tol": params.pos_constraint_tol if params else 0.0,
-                        "pos_cost_weight": params.pos_cost_weight if params else 0.0
-                    }
-                }
-            }
-            
-            response = service.call(request)
-            if response.get('success', False):
-                return response['hand_poses']['left_pose']['joint_angles'] + response['hand_poses']['right_pose']['joint_angles']
-            return None
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-            return None
-
-    def arm_fk(self, q: list) -> Tuple[KuavoPose, KuavoPose]:
-        try:
-            websocket = WebSocketKuavoSDK()
-            service = roslibpy.Service(websocket.client, '/ik/fk_srv', 'motion_capture_ik/fkSrv')
-            request = {"q": q}
-            response = service.call(request)
-            
-            if response.get('success', False):
-                left_pose = KuavoPose(
-                    position=response['hand_poses']['left_pose']['pos_xyz'],
-                    orientation=response['hand_poses']['left_pose']['quat_xyzw']
-                )
-                right_pose = KuavoPose(
-                    position=response['hand_poses']['right_pose']['pos_xyz'],
-                    orientation=response['hand_poses']['right_pose']['quat_xyzw']
-                )
-                return left_pose, right_pose
-            return None
-        except Exception as e:
-            SDKLogger.error(f"Service call failed: {e}")
-            return None
-
 """
     Kuavo Robot Control 
 """
@@ -1346,39 +613,19 @@ class KuavoRobotControl:
         SDKLogger.debug(f"Control robot head: {yaw}, {pitch}")
         return self.kuavo_head_control.pub_control_robot_head(yaw, pitch)
     
-    def enable_head_tracking(self, target_id: int)->bool:
-        """Enable the head tracking for a specific tag ID.
-        
-        Args:
-            target_id: The ID of the tag to track
-            
-        Returns:
-            bool: True if successful, False otherwise
-        """
-        SDKLogger.debug(f"Enable head tracking: {target_id}")
-        return self.kuavo_head_control.srv_enable_head_tracking(target_id)
-    
-    def disable_head_tracking(self)->bool:
-        """Disable the head tracking.
-        
-        Returns:
-            bool: True if successful, False otherwise
-        """
-        SDKLogger.debug(f"Disable head tracking")
-        return self.kuavo_head_control.srv_disable_head_tracking()
-    
-    def control_robot_arm_joint_positions(self, joint_data:list)->bool:
+
+    def control_robot_arm_traj(self, joint_data:list)->bool:
         """
-            Control robot arm joint positions
+            Control robot arm trajectory
             Arguments:
                 - joint_data: list of joint data (degrees)
         """
         # SDKLogger.debug(f"[ROS] Control robot arm trajectory: {joint_data}")
         return self.kuavo_arm_control.pub_control_robot_arm_traj(joint_data)
     
-    def control_robot_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
+    def control_robot_arm_target_poses(self, times:list, joint_q:list)->bool:
         """
-            Control robot arm joint trajectory
+            Control robot arm target poses
             Arguments:
                 - times: list of times (seconds)
                 - joint_q: list of joint data (degrees)
@@ -1390,58 +637,6 @@ class KuavoRobotControl:
         
         return self.kuavo_arm_control.pub_arm_target_poses(times=times, joint_q=joint_q)
     
-    def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:
-        """
-            Control robot end effector pose
-            Arguments:
-                - left_pose: left end effector pose
-                - right_pose: right end effector pose
-                - frame: frame of the end effector pose, 0: keep current frame, 1: world frame, 2: local frame, 3: VR frame, 4: manipulation world frame
-        """
-        return self.kuavo_arm_control.pub_end_effector_pose_cmd(left_pose, right_pose, frame)
-    
-    def change_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame)->bool:
-        """
-            Change manipulation mpc frame
-            Arguments:
-                - frame: frame of the manipulation mpc
-        """
-        return self.kuavo_arm_control.srv_change_manipulation_mpc_frame(frame)
-    
-    def change_manipulation_mpc_ctrl_mode(self, ctrl_mode: KuavoManipulationMpcCtrlMode)->bool:
-        """
-            Change manipulation mpc control mode
-            Arguments:
-                - control_mode: control mode of the manipulation mpc
-        """
-        return self.kuavo_arm_control.srv_change_manipulation_mpc_ctrl_mode(ctrl_mode)
-    
-    def change_manipulation_mpc_control_flow(self, ctrl_flow: KuavoManipulationMpcControlFlow)->bool:
-        """
-            Change manipulation mpc wbc arm traj control mode, control signal will be sent to wbc directly
-            Arguments:
-                - control_mode: control mode of the manipulation mpc wbc arm traj
-        """
-        return self.kuavo_arm_control.srv_change_manipulation_mpc_control_flow(ctrl_flow)
-    
-    def get_manipulation_mpc_ctrl_mode(self)->KuavoManipulationMpcCtrlMode:
-        """
-            Get manipulation mpc control mode
-        """
-        return self.kuavo_arm_control.srv_get_manipulation_mpc_ctrl_mode()
-    
-    def get_manipulation_mpc_frame(self)-> KuavoManipulationMpcFrame:
-        """
-            Get manipulation mpc frame
-        """
-        return self.kuavo_arm_control.srv_get_manipulation_mpc_frame()  
-    
-    def get_manipulation_mpc_control_flow(self)->KuavoManipulationMpcControlFlow:
-        """
-            Get manipulation mpc wbc arm traj control mode
-        """
-        return self.kuavo_arm_control.srv_get_manipulation_mpc_control_flow()   
-    
     def change_robot_arm_ctrl_mode(self, mode:KuavoArmCtrlMode)->bool:
         """
             Change robot arm control mode
@@ -1483,28 +678,6 @@ class KuavoRobotControl:
         com_msg.angular.y = pitch
         return self.kuavo_motion_control.pub_cmd_pose(com_msg)
 
-    def control_command_pose_world(self, target_pose_x:float, target_pose_y:float, target_pose_z:float, target_pose_yaw:float)->bool:
-        """
-            odom下的机器人cmd_pose_world
-        """
-        com_msg = Twist()
-        com_msg.linear.x = target_pose_x
-        com_msg.linear.y = target_pose_y
-        com_msg.linear.z = target_pose_z
-        com_msg.angular.z = target_pose_yaw
-        return self.kuavo_motion_control.pub_cmd_pose_world(com_msg)
-
-    def control_command_pose(self, target_pose_x:float, target_pose_y:float, target_pose_z:float, target_pose_yaw:float)->bool:
-        """
-            base_link下的机器人cmd_pose
-        """
-        com_msg = Twist()
-        com_msg.linear.x = target_pose_x
-        com_msg.linear.y = target_pose_y
-        com_msg.linear.z = target_pose_z
-        com_msg.angular.z = target_pose_yaw
-        return self.kuavo_motion_control.pub_cmd_pose(com_msg)
-
     def step_control(self, body_poses:list, dt:float, is_left_first_default:bool=True, collision_check:bool=True)->bool:
         """
             Step control
@@ -1525,226 +698,6 @@ class KuavoRobotControl:
         msg = get_multiple_steps_msg(body_poses, dt, is_left_first_default, collision_check)
         return self.kuavo_motion_control.pub_step_ctrl(msg)
 """ ------------------------------------------------------------------------------"""    
-
-
-class KuavoRobotControlWebsocket:
-    _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._initialized = True
-                self.kuavo_eef_control = None
-                self.kuavo_head_control = ControlRobotHeadWebsocket()
-                self.kuavo_arm_control = ControlRobotArmWebsocket()
-                self.kuavo_motion_control = ControlRobotMotionWebsocket()
-                self.kuavo_arm_ik_fk = KuavoRobotArmIKFKWebsocket()
-            except Exception as e:
-                SDKLogger.error(f"Failed to initialize KuavoRobotControlWebsocket: {e}")
-                raise
-
-    def initialize(self, eef_type:str=None, debug:bool=False, timeout:float=1.0)-> Tuple[bool, str]:
-        try:
-            # init eef control
-            if eef_type is None:
-                self.kuavo_eef_control = None
-            else:
-                self.kuavo_eef_control = ControlEndEffectorWebsocket(eef_type=eef_type)
-            
-            connect_success = True
-            err_msg = ''
-            if not self.kuavo_arm_control.connect(timeout):
-                connect_success = False
-                err_msg = "Failed to connect to arm control topics, \n"
-            if not self.kuavo_head_control.connect(timeout):
-                connect_success = False
-                err_msg += "Failed to connect to head control topics, \n"
-            if not self.kuavo_motion_control.connect(timeout):
-                err_msg += "Failed to connect to motion control topics, \n"
-                connect_success = False
-
-            if self.kuavo_eef_control is not None and not self.kuavo_eef_control.connect(timeout):
-                connect_success = False
-                err_msg += "Failed to connect to end effector control topics."
-
-            if connect_success:
-                err_msg = 'success'
-            return connect_success, err_msg
-        except Exception as e:
-            SDKLogger.error(f"Failed to initialize KuavoRobotControlWebsocket: {e}")
-            return False, str(e)
-    
-    """ End Effector Control"""
-    def control_robot_dexhand(self, left_position:list, right_position:list)->bool:
-        """
-            Control robot dexhand
-            Args:
-                left_position: list of 6 floats between 0 and 100
-                right_position: list of 6 floats between 0 and 100
-        """
-        if self.kuavo_eef_control is None:
-            SDKLogger.error("End effector control is not initialized.")
-            return False
-        
-        if len(left_position) != 6 or len(right_position) != 6:
-            raise ValueError("Position lists must have a length of 6.")
-        
-        for position in left_position + right_position:
-            if position < 0.0 or position > 100.0:
-                raise ValueError("All position values must be in the range [0.0, 100.0].")    
-        
-        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
-            Arguments:
-                - gestures: list of dicts with keys 'gesture_name' and 'hand_side'
-                 e.g. [{'gesture_name': 'fist', 'hand_side': 0},]
-        """
-        if self.kuavo_eef_control is None:
-            SDKLogger.warn("End effectors control is not initialized.")
-            return False
-        return self.kuavo_eef_control.srv_execute_gesture(gestures)
-
-    def get_gesture_names(self)->list:
-        """
-            Get the names of all gestures.
-        """
-        if self.kuavo_eef_control is None:
-            SDKLogger.warn("End effectors control is not initialized.")
-            return []
-        return self.kuavo_eef_control.srv_get_gesture_names()
-
-    def control_leju_claw(self, postions:list, velocities:list=[90, 90], torques:list=[1.0, 1.0]) ->bool:
-        """
-            Control leju claw
-            Arguments:
-                - postions: list of positions for left and right claw
-                - velocities: list of velocities for left and right claw
-                - torques: list of torques for left and right claw
-        """
-        if self.kuavo_eef_control is None:
-            SDKLogger.warn("End effectors control is not initialized.")
-            return False
-        SDKLogger.debug(f"Control leju claw: {postions}, {velocities}, {torques}")
-        if len(postions) != 2 or len(velocities) != 2 or len(torques) != 2:
-                raise ValueError("Position, velocity, and torque lists must have a length of 2.")
-        return self.kuavo_eef_control.srv_control_leju_claw(postions, velocities, torques)
-
-    def control_robot_head(self, yaw:float, pitch:float)->bool:
-        """
-            Control robot head
-            Arguments:
-                - yaw: yaw angle, radian
-                - pitch: pitch angle, radian
-        """
-        SDKLogger.debug(f"Control robot head: {yaw}, {pitch}")
-        return self.kuavo_head_control.pub_control_robot_head(yaw, pitch)
-    
-    def control_robot_arm_traj(self, joint_data:list)->bool:
-        """
-            Control robot arm trajectory
-            Arguments:
-                - joint_data: list of joint data (degrees)
-        """
-        return self.kuavo_arm_control.pub_control_robot_arm_traj(joint_data)
-    
-    def control_robot_arm_target_poses(self, times:list, joint_q:list)->bool:
-        """
-            Control robot arm target poses
-            Arguments:
-                - times: list of times (seconds)
-                - joint_q: list of joint data (degrees)
-        """
-        if len(times) != len(joint_q):
-            raise ValueError("Times and joint_q must have the same length.")
-        elif len(times) == 0:
-            raise ValueError("Times and joint_q must not be empty.")
-        
-        return self.kuavo_arm_control.pub_arm_target_poses(times=times, joint_q=joint_q)
-    
-    def change_robot_arm_ctrl_mode(self, mode:KuavoArmCtrlMode)->bool:
-        """
-            Change robot arm control mode
-            Arguments:
-                - mode: arm control mode
-        """
-        SDKLogger.debug(f"[WebSocket] Change robot arm control mode: {mode}")
-        return self.kuavo_arm_control.srv_change_arm_ctrl_mode(mode)
-    
-    def get_robot_arm_ctrl_mode(self)->int:
-        """
-            Get robot arm control mode
-        """
-        return self.kuavo_arm_control.srv_get_arm_ctrl_mode()
-    
-    def arm_ik(self, left_pose: KuavoPose, right_pose: KuavoPose, 
-               left_elbow_pos_xyz: list = [0.0, 0.0, 0.0],  
-               right_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
-               arm_q0: list = None, params: KuavoIKParams=None) -> list:
-        return self.kuavo_arm_ik_fk.arm_ik(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
-
-    def arm_fk(self, q: list) -> Tuple[KuavoPose, KuavoPose]:
-        return self.kuavo_arm_ik_fk.arm_fk(q)
-    
-    """ Motion """
-    def robot_stance(self)->bool:
-        return self.kuavo_motion_control.pub_stance_command()
-
-    def robot_trot(self)->bool:
-        return self.kuavo_motion_control.pub_trot_command()
-    
-    def robot_walk(self, linear_x:float, linear_y:float, angular_z:float)->bool:
-        return self.kuavo_motion_control.pub_cmd_vel(linear_x, linear_y, angular_z)
-    
-    def control_torso_height(self, height:float, pitch:float=0.0)->bool:
-        return self.kuavo_motion_control.pub_cmd_pose(height, pitch)
-
-    def step_control(self, body_poses:list, dt:float, is_left_first_default:bool=True, collision_check:bool=True)->bool:
-        """
-            Step control
-            Arguments:
-                - body_poses: list of body poses (x, y, z, yaw), meters and degrees
-                - dt: time step (seconds)
-                - is_left_first_default: whether to start with left foot
-                - collision_check: whether to check for collisions
-        """
-        if len(body_poses) == 0:
-            raise ValueError("Body poses must not be empty.")
-        if dt <= 0.0:
-            raise ValueError("Time step must be greater than 0.0.")
-        for bp in body_poses:
-            if len(bp) != 4:
-                raise ValueError("Body pose must have 4 elements: [x, y, z, yaw]")
-        
-        msg = get_multiple_steps_msg(body_poses, dt, is_left_first_default, collision_check)
-        return self.kuavo_motion_control.pub_step_ctrl(msg)
-
-# if GlobalConfig.use_websocket:
-#     kuavo_robot_control = KuavoRobotControlWebsocket()
-# else:
-#     kuavo_robot_control = KuavoRobotControl()
-
 def euler_to_rotation_matrix(yaw, pitch, roll):
     # 计算各轴的旋转矩阵
     R_yaw = np.array([[np.cos(yaw), -np.sin(yaw), 0],
@@ -1858,17 +811,4 @@ def get_multiple_steps_msg(body_poses:list, dt:float, is_left_first:bool=True, c
     # print("foot_traj:", foot_traj)
     # print("torso_traj:", torso_traj)
     return get_foot_pose_traj_msg(time_traj, foot_idx_traj, foot_traj, torso_traj)
-""" ------------------------------------------------------------------------------"""
-
-
-# if __name__ == "__main__":
-#     control = KuavoRobotControl()
-#     control.change_manipulation_mpc_frame(KuavoManipulationMpcFrame.KeepCurrentFrame)
-#     control.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.ArmOnly)
-#     control.change_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.DirectToWbc)
-#     print(control.get_manipulation_mpc_ctrl_mode())
-#     print(control.get_manipulation_mpc_frame())
-#     print(control.get_manipulation_mpc_control_flow())
-#     control.change_manipulation_mpc_frame(KuavoManipulationMpcFrame.WorldFrame)
-#     control.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl)
-#     control.control_robot_end_effector_pose(KuavoPose(position=[0.3, 0.4, 0.9], orientation=[0.0, 0.0, 0.0, 1.0]), KuavoPose(position=[0.3, -0.5, 1.0], orientation=[0.0, 0.0, 0.0, 1.0]), KuavoManipulationMpcFrame.WorldFrame)
+""" ------------------------------------------------------------------------------"""