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

kuavo_humanoid_sdk/kuavo/core/ros_env.py

@@ -2,15 +2,10 @@
 # coding: utf-8
 
 import os
-try:
-    import rospy
-except ImportError:
-    pass
+import rospy
 import subprocess
 import atexit
 from kuavo_humanoid_sdk.common.logger import SDKLogger
-from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
-import roslibpy
 
 class KuavoROSEnv:
     _instance = None
@@ -237,226 +232,3 @@ class KuavoROSEnv:
         except Exception as e:
             SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
             return False
-
-class KuavoROSEnvWebsocket:
-    _instance = None
-    _processes = []  # Store all subprocess instances
-
-    def __new__(cls):
-        if cls._instance is None:
-            cls._instance = super(KuavoROSEnvWebsocket, cls).__new__(cls)
-            cls._instance._initialized = False
-            cls._instance._init_success = False  # Add flag to track successful Init() call
-            # Register cleanup handler
-            atexit.register(cls._cleanup_processes)
-        return cls._instance
-
-    def __init__(self):
-        if not self._initialized:
-            self._initialized = True
-            self.websocket = None
-
-    @classmethod
-    def _cleanup_processes(cls):
-        """Cleanup all registered processes on exit"""
-        for process in cls._processes:
-            if process.poll() is None:  # Process is still running
-                process.terminate()
-                try:
-                    process.wait(timeout=3)  # Wait for process to terminate
-                except subprocess.TimeoutExpired:
-                    process.kill()  # Force kill if not terminated
-        cls._processes.clear()
-
-    def _get_kuavo_ws_root(self) -> str:
-        # For WebSocket version, we'll use environment variable instead of ROS param
-        model_path = os.environ.get('KUAVO_MODEL_PATH')
-        if not model_path:
-            raise Exception("KUAVO_MODEL_PATH environment variable not found")
-        
-        if not os.path.exists(model_path):
-            raise Exception(f"Model path {model_path} not found")
-            
-        # ws 
-        return model_path.replace('/src/kuavo_assets/models', '')
-    
-    def Init(self) -> bool:
-        """
-        Initialize the WebSocket environment.
-        Raises:
-            Exception: If the KUAVO_MODEL_PATH environment variable is not found or the path is not valid.
-        """
-        # if generate docs, skip init.
-        if 'GEN_KUAVO_HUMANOID_SDK_DOCS' in os.environ:
-            return True
-
-        # Return directly if already initialized successfully
-        if self._init_success:
-            return True
-        
-        # Check if WebSocket server is running
-        try:
-            
-            self.websocket = WebSocketKuavoSDK()
-            if not self.websocket.client.is_connected:
-                print(f"\033[31m\nError: Can't connect to WebSocket server. Please ensure the server is running.\033[0m"
-                      "\nMaybe manually launch the app first?"
-                      "\n - for example(sim): roslaunch humanoid_controller load_kuavo_mujoco_sim.launch, "
-                      "\n - for example(real): roslaunch humanoid_controller load_kuavo_real.launch\n")
-                exit(1)
-        except Exception as e:
-            print(f"\033[31m\nError: Failed to connect to WebSocket server: {e}\033[0m")
-            exit(1)
-        
-        # Only check nodes exist when Init SDK, if not, tips user manually launch nodes.
-        deps_nodes = ['/humanoid_gait_switch_by_name']
-        for node in deps_nodes:
-            if not self.check_rosnode_exists(node):
-                print(f"\033[31m\nError: Node {node} not found. Please launch it manually.\033[0m")
-                exit(1)
-                
-        self._init_success = True  # Set flag after successful initialization
-
-        return True
-    
-    def _launch_ros_node(self, node_name, launch_cmd, log_name):
-        """Launch a ROS node with the given command and log the output.
-        
-        Args:
-            node_name (str): Name of the node to launch
-            launch_cmd (str): Full launch command including source and roslaunch
-            log_name (str): Name for the log file
-            
-        Raises:
-            Exception: If node launch fails
-        """
-        # Launch in background and check if successful
-        try:
-            os.makedirs('/var/log/kuavo_humanoid_sdk', exist_ok=True)
-            log_path = f'/var/log/kuavo_humanoid_sdk/{log_name}.log'
-        except PermissionError:
-            os.makedirs('log/kuavo_humanoid_sdk', exist_ok=True)
-            log_path = f'log/kuavo_humanoid_sdk/{log_name}.log'
-            
-        with open(log_path, 'w') as log_file:
-            process = subprocess.Popen(launch_cmd, shell=True, executable='/bin/bash', stdout=log_file, stderr=log_file)
-            self._processes.append(process)  # Add process to tracking list
-            
-        if process.returncode is not None and process.returncode != 0:
-            raise Exception(f"Failed to launch {node_name}, return code: {process.returncode}")
-
-        SDKLogger.info(f"{node_name} launched successfully")
-
-    def _get_setup_file(self, ws_root=None):
-        """Get the appropriate ROS setup file path based on shell type.
-        
-        Args:
-            ws_root (str, optional): ROS workspace root path. If None, uses ROS_WORKSPACE.
-            
-        Returns:
-            str: Path to the setup file
-            
-        Raises:
-            Exception: If setup file not found
-        """
-        is_zsh = 'zsh' in os.environ.get('SHELL', '')
-        
-        if ws_root is None:
-            ws_root = os.environ['ROS_WORKSPACE']
-            
-        setup_files = {
-            'zsh': os.path.join(ws_root, 'devel/setup.zsh'),
-            'bash': os.path.join(ws_root, 'devel/setup.bash')
-        }
-        
-        setup_file = setup_files['zsh'] if is_zsh else setup_files['bash']
-        if not os.path.exists(setup_file):
-            setup_file = setup_file.replace('devel', 'install')
-            if not os.path.exists(setup_file):
-                raise Exception(f"Setup file not found in either devel or install: {setup_file}")
-                
-        return setup_file
-
-    def launch_ik_node(self):
-        # nodes: /arms_ik_node
-        # services: /ik/two_arm_hand_pose_cmd_srv, /ik/fk_srv
-        try:
-            if not self.websocket or not self.websocket.client.is_connected:
-                raise Exception("WebSocket server is not running")
-        except Exception as e:
-            raise Exception(f"WebSocket server is not running: {e}")
-
-        # Check if IK node and services exist
-        try:
-            # Check if arms_ik_node is running using roslibpy
-            service = roslibpy.Service(self.websocket.client, '/rosnode/list', 'rosapi/Nodes')
-            response = service.call({})
-            nodes = response.get('nodes', [])
-            
-            if '/arms_ik_node' not in nodes:
-                # Launch IK node if not running
-                kuavo_ws_root = self._get_kuavo_ws_root()
-                setup_file = self._get_setup_file(kuavo_ws_root)
-                source_cmd = f"source {setup_file}"
-                
-                # Get robot version from environment variable
-                robot_version = os.environ.get('ROBOT_VERSION')
-                if robot_version is None:
-                    raise Exception("Failed to get ROBOT_VERSION from environment variables")
-                
-                # Launch IK node
-                launch_cmd = f"roslaunch motion_capture_ik ik_node.launch robot_version:={robot_version}"
-                full_cmd = f"{source_cmd} && {launch_cmd}"
-                
-                self._launch_ros_node('IK node', full_cmd, 'launch_ik')
-
-            return True
-
-        except Exception as e:
-            raise Exception(f"Failed to verify IK node and services: {e}")
-        
-
-    def launch_gait_switch_node(self)-> bool:
-        """Verify that the gait switch node is running, launch if not."""
-        try:
-            # Check if node exists using roslibpy
-            service = roslibpy.Service(self.websocket.client, '/rosnode/list', 'rosapi/Nodes')
-            response = service.call({})
-            nodes = response.get('nodes', [])
-            
-            if '/humanoid_gait_switch_by_name' not in nodes:
-                kuavo_ws_root = self._get_kuavo_ws_root()
-                setup_file = self._get_setup_file(kuavo_ws_root)
-                source_cmd = f"source {setup_file}"
-    
-                # Launch gait switch node
-                launch_cmd = "roslaunch humanoid_interface_ros humanoid_switch_gait.launch"
-                full_cmd = f"{source_cmd} && {launch_cmd}"
-                
-                self._launch_ros_node('Gait switch node', full_cmd, 'launch_gait_switch')
-
-        except Exception as e:
-            raise Exception(f"Failed to launch gait_switch node: {e}")
-        
-    @staticmethod
-    def check_rosnode_exists(node_name):
-        """Check if a ROS node is running using roslibpy.
-        
-        Args:
-            node_name (str): Name of the node to check
-            
-        Returns:
-            bool: True if node is running, False otherwise
-        """
-        websocket = WebSocketKuavoSDK()
-        try:
-            if not websocket or not websocket.client.is_connected:
-                return False
-                
-            service = roslibpy.Service(websocket.client, '/rosapi/nodes', 'rosapi/Nodes')
-            response = service.call({})
-            nodes = response.get('nodes', [])
-            return node_name in nodes
-        except Exception as e:
-            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
-            return False

kuavo_humanoid_sdk/kuavo/dexterous_hand.py

@@ -4,8 +4,7 @@ from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.end_effector import EndEffector
 from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState, KuavoDexHandTouchState
 from kuavo_humanoid_sdk.kuavo.core.dex_hand_control import DexHandControl
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
-from kuavo_humanoid_sdk.common.global_config import GlobalConfig
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
 
 class DexterousHand(EndEffector):
     def __init__(self):
@@ -13,10 +12,7 @@ class DexterousHand(EndEffector):
                        'r_thumb', 'r_thumb_aux', 'r_index', 'r_middle', 'r_ring', 'r_pinky',]
         super().__init__(joint_names=joint_names)
         self.dex_hand_control = DexHandControl()
-        if GlobalConfig.use_websocket:
-            self._rb_state = KuavoRobotStateCoreWebsocket()
-        else:
-            self._rb_state = KuavoRobotStateCore()
+        self._rb_state = KuavoRobotStateCore()
 
     def control(self, target_positions:list, target_velocities:list=None, target_torques:list=None)->bool:
         """Set the position of the hand.

kuavo_humanoid_sdk/kuavo/leju_claw.py

@@ -5,17 +5,13 @@ from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.end_effector import EndEffector
 from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState
 from kuavo_humanoid_sdk.kuavo.core.leju_claw_control import LejuClawControl
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
-from kuavo_humanoid_sdk.common.global_config import GlobalConfig
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
 
 class LejuClaw(EndEffector):
     def __init__(self):
         super().__init__(joint_names=['left_claw', 'right_claw'])
         self.leju_claw_control = LejuClawControl()
-        if GlobalConfig.use_websocket:
-            self._rb_state = KuavoRobotStateCoreWebsocket()
-        else:
-            self._rb_state = KuavoRobotStateCore()
+        self._rb_state = KuavoRobotStateCore()
 
     def control(self, target_positions: list, target_velocities:list=None, target_torques: list=None)->bool:
         """Control the claws to grip.

kuavo_humanoid_sdk/kuavo/robot.py

@@ -1,17 +1,18 @@
 #!/usr/bin/env python3
 # coding: utf-8
-from kuavo_humanoid_sdk.common.global_config import GlobalConfig
-
-from kuavo_humanoid_sdk.kuavo.core.ros_env import KuavoROSEnv, KuavoROSEnvWebsocket
-from kuavo_humanoid_sdk.interfaces.robot import RobotBase
-from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
-from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
-from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
-
-from typing import Tuple
-from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
-from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
-from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
+from kuavo_humanoid_sdk.kuavo.core.ros_env import KuavoROSEnv
+kuavo_ros_env = KuavoROSEnv()
+if not kuavo_ros_env.Init():
+    raise RuntimeError("Failed to initialize ROS environment")
+else:
+    from typing import Tuple
+    from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
+    from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
+    from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
+    from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+    from kuavo_humanoid_sdk.interfaces.robot import RobotBase
+    from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams
+    from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
 
 """
 Kuavo SDK - Python Interface for Kuavo Robot Control
@@ -44,30 +45,19 @@ class KuavoSDK:
        pass
 
     @staticmethod   
-    def Init(options:int=Options.Normal, log_level: str = "INFO", 
-             websocket_mode: bool = False, websocket_host='127.0.0.1', websocket_port=9090, websocket_timeout=5)-> bool:
+    def Init(options:int=Options.Normal, log_level: str = "INFO")-> bool:
         """Initialize the SDK.
+        
         Args:
             log_level (str): The logging level to use. Can be "ERROR", "WARN", "INFO", "DEBUG".
                 Defaults to "INFO".
-            websocket_mode (bool): Whether to use websocket mode for ROS communication. If True,
-                the SDK will connect to ROS through websocket instead of direct ROS connection.
-                Defaults to False.
-            websocket_host (str): The host address of the rosbridge websocket server when using
-                websocket mode. Defaults to "127.0.0.1".
-            websocket_port (int): The port number of the rosbridge websocket server when using
-                websocket mode. Defaults to 9090.
-            websocket_timeout (int): The timeout for the websocket connection. Defaults to 5.
+                
         Returns:
             bool: True if initialization is successful, False otherwise.
             
         Raises:
             RuntimeError: If the initialization fails.
         """
-        GlobalConfig.use_websocket = websocket_mode
-        GlobalConfig.websocket_host = websocket_host
-        GlobalConfig.websocket_port = websocket_port
-        GlobalConfig.websocket_timeout = websocket_timeout
         SDKLogger.setLevel(log_level.upper())
         # Initialize core components, connect ROS Topics...
         kuavo_core = KuavoRobotCore()
@@ -75,18 +65,12 @@ class KuavoSDK:
             debug = True
         else:
             debug = False   
-        # Check if IK option is enabled
-        if GlobalConfig.use_websocket:
-            if options & KuavoSDK.Options.WithIK:
-                if not KuavoROSEnvWebsocket.check_rosnode_exists('/arms_ik_node'):
-                    print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
-                    exit(1)
-        else:
-            if options & KuavoSDK.Options.WithIK:
-                if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
-                    print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
-                    exit(1)
 
+        # Check if IK option is enabled
+        if options & KuavoSDK.Options.WithIK:
+            if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
+                print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
+                exit(1)
 
         if not kuavo_core.initialize(debug=debug):
             SDKLogger.error("[SDK] Failed to initialize core components.")
@@ -102,16 +86,11 @@ class KuavoSDK:
 class KuavoRobot(RobotBase):
     def __init__(self):
         super().__init__(robot_type="kuavo")
-        
-        if not GlobalConfig.use_websocket:
-            kuavo_ros_env = KuavoROSEnv()
-            if not kuavo_ros_env.Init():
-                raise RuntimeError("Failed to initialize ROS environment")
-
         self._robot_info = KuavoRobotInfo()
         self._robot_arm  = KuavoRobotArm()
         self._robot_head = KuavoRobotHead()
         self._kuavo_core = KuavoRobotCore()
+
     def stance(self)->bool:
         """Put the robot into 'stance' mode.
         
@@ -219,46 +198,6 @@ class KuavoRobot(RobotBase):
             raise ValueError(f"[Robot] target_pose length must be 4 (x, y, z, yaw), but got {len(target_pose)}")
 
         return self._kuavo_core.step_control(target_pose, dt, is_left_first_default, collision_check)
-
-    def control_command_pose(self, target_pose_x: float, target_pose_y: float, target_pose_z: float, target_pose_yaw: float) -> bool:
-        """Control robot pose in base_link frame.
-        
-        Args:
-            target_pose_x (float): The target x position in meters.
-            target_pose_y (float): The target y position in meters.
-            target_pose_z (float): The target z position in meters.
-            target_pose_yaw (float): The target yaw angle in radians.
-            
-        Returns:
-            bool: True if the command was sent successfully, False otherwise.
-            
-        Raises:
-            RuntimeError: If the robot is not in stance state when trying to control pose.
-            
-        Note:
-            This command changes the robot state to 'command_pose'.
-        """
-        return self._kuavo_core.control_command_pose(target_pose_x, target_pose_y, target_pose_z, target_pose_yaw)
-
-    def control_command_pose_world(self, target_pose_x: float, target_pose_y: float, target_pose_z: float, target_pose_yaw: float) -> bool:
-        """Control robot pose in odom (world) frame.
-        
-        Args:
-            target_pose_x (float): The target x position in meters.
-            target_pose_y (float): The target y position in meters.
-            target_pose_z (float): The target z position in meters.
-            target_pose_yaw (float): The target yaw angle in radians.
-            
-        Returns:
-            bool: True if the command was sent successfully, False otherwise.
-            
-        Raises:
-            RuntimeError: If the robot is not in stance state when trying to control pose.
-            
-        Note:
-            This command changes the robot state to 'command_pose_world'.
-        """
-        return self._kuavo_core.control_command_pose_world(target_pose_x, target_pose_y, target_pose_z, target_pose_yaw)
     
     def control_head(self, yaw: float, pitch: float)->bool:
         """Control the head of the robot.
@@ -272,16 +211,6 @@ class KuavoRobot(RobotBase):
         """
         return self._robot_head.control_head(yaw=yaw, pitch=pitch)
     
-    def enable_head_tracking(self, target_id: int)->bool:
-        """Enable the head tracking.
-        """
-        return self._robot_head.enable_head_tracking(target_id)
-    
-    def disable_head_tracking(self)->bool:
-        """Disable the head tracking.
-        """
-        return self._robot_head.disable_head_tracking()
-    
     """ Robot Arm Control """
     def arm_reset(self)->bool:
         """Reset the robot arm.
@@ -290,16 +219,8 @@ class KuavoRobot(RobotBase):
             bool: True if the arm is reset successfully, False otherwise.
         """
         return self._robot_arm.arm_reset()
-    
-    def manipulation_mpc_reset(self)->bool:
-        """Reset the robot arm.
         
-        Returns:
-            bool: True if the arm is reset successfully, False otherwise.
-        """
-        return self._robot_arm.manipulation_mpc_reset()
-    
-    def control_arm_joint_positions(self, joint_positions:list)->bool:
+    def control_arm_position(self, joint_positions:list)->bool:
         """Control the position of the arm.
         
         Args:
@@ -317,9 +238,9 @@ class KuavoRobot(RobotBase):
             print("The length of the position list must be equal to the number of DOFs of the arm.")
             return False
         
-        return self._robot_arm.control_arm_joint_positions(joint_positions)
+        return self._robot_arm.control_arm_position(joint_positions)
     
-    def control_arm_joint_trajectory(self, times:list, q_frames:list)->bool:
+    def control_arm_target_poses(self, times:list, q_frames:list)->bool:
         """Control the target poses of the robot arm.
         
         Args:
@@ -339,7 +260,7 @@ class KuavoRobot(RobotBase):
             This is an asynchronous interface. The function returns immediately after sending the command.
             Users need to wait for the motion to complete on their own.
         """
-        return self._robot_arm.control_arm_joint_trajectory(times, q_frames)
+        return self._robot_arm.control_arm_target_poses(times, q_frames)
 
     def set_fixed_arm_mode(self) -> bool:
         """Freezes the robot arm.
@@ -365,30 +286,6 @@ class KuavoRobot(RobotBase):
         """
         return self._robot_arm.set_external_control_arm_mode()
     
-    def set_manipulation_mpc_mode(self, ctrl_mode: KuavoManipulationMpcCtrlMode) -> bool:
-        """
-        Set the manipulation mpc mode.
-        Returns:
-            bool: True if the manipulation mpc mode is set successfully, False otherwise.
-        """
-        return self._robot_arm.set_manipulation_mpc_mode(ctrl_mode)
-    
-    def set_manipulation_mpc_control_flow(self, control_flow: KuavoManipulationMpcControlFlow) -> bool:
-        """
-        Set the manipulation mpc control flow.
-        Returns:
-            bool: True if the manipulation mpc control flow is set successfully, False otherwise.
-        """ 
-        return self._robot_arm.set_manipulation_mpc_control_flow(control_flow)
-
-    def set_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame) -> bool:
-        """
-        Set the manipulation mpc frame.
-        Returns:
-            bool: True if the manipulation mpc frame is set successfully, False otherwise.
-        """
-        return self._robot_arm.set_manipulation_mpc_frame(frame)
-    
     """ Arm Forward kinematics && Arm Inverse kinematics """
     def arm_ik(self, 
                left_pose: KuavoPose, 
@@ -436,24 +333,4 @@ class KuavoRobot(RobotBase):
         Warning:
             This function requires initializing the SDK with the :attr:`KuavoSDK.Options.WithIK`.        
         """
-        return self._robot_arm.arm_fk(q)
-
-    def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:
-        """Control the end effector pose of the robot arm.
-        
-        Args:
-            left_pose (KuavoPose): Pose of the robot left arm, xyz and quat.
-            right_pose (KuavoPose): Pose of the robot right arm, xyz and quat.
-            frame (KuavoManipulationMpcFrame): Frame of the robot arm.
-            
-        Returns:
-            bool: True if the end effector pose is controlled successfully, False otherwise.
-        """
-        return self._robot_arm.control_robot_end_effector_pose(left_pose, right_pose, frame)
-
-if __name__ == "__main__":
-    robot = KuavoRobot()
-    robot.set_manipulation_mpc_mode(KuavoManipulationMpcCtrlMode.ArmOnly)
-    robot.set_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.DirectToWbc)
-    robot.set_manipulation_mpc_frame(KuavoManipulationMpcFrame.WorldFrame)
-    robot.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)
+        return self._robot_arm.arm_fk(q)

kuavo_humanoid_sdk/kuavo/robot_arm.py

@@ -3,7 +3,7 @@
 
 import math
 from typing import Tuple
-from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose
 from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
 from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
 
@@ -14,11 +14,8 @@ class KuavoRobotArm:
     
     def arm_reset(self)-> bool:
         return self._kuavo_core.robot_arm_reset()
-    
-    def manipulation_mpc_reset(self)-> bool:
-        return self._kuavo_core.robot_manipulation_mpc_reset()
         
-    def control_arm_joint_positions(self, joint_position:list)->bool:
+    def control_arm_position(self, joint_position:list)->bool:
         """
             Control the position of the robot arm joint.
             Args:
@@ -38,9 +35,9 @@ class KuavoRobotArm:
             if abs(pos) > math.pi:
                 raise ValueError(f"Joint position {pos} rad exceeds ±π rad (±180 deg) limit")
 
-        return self._kuavo_core.control_robot_arm_joint_positions(joint_data=joint_position)
+        return self._kuavo_core.control_robot_arm_traj(joint_data=joint_position)
 
-    def control_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
+    def control_arm_target_poses(self, times:list, q_frames:list)->bool:
         """
             Control the target poses of the robot arm.
             Args:
@@ -54,12 +51,12 @@ class KuavoRobotArm:
             Returns:
                 bool: True if the control was successful, False otherwise.    
         """
-        if len(times) != len(joint_q):
+        if len(times) != len(q_frames):
             raise ValueError("Invalid input. times and joint_q must have thesame length.")
         
         # Check if joint positions are within ±180 degrees (±π radians)
         q_degs = []
-        for q in joint_q:
+        for q in q_frames:
             if any(abs(pos) > math.pi for pos in q):
                 raise ValueError("Joint positions must be within ±π rad (±180 deg)")
             if len(q) != self._robot_info.arm_joint_dof:
@@ -67,19 +64,7 @@ class KuavoRobotArm:
             # Convert joint positions from radians to degrees
             q_degs.append([(p * 180.0 / math.pi) for p in q])
 
-        return self._kuavo_core.control_robot_arm_joint_trajectory(times=times, joint_q=q_degs)
-
-    def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:
-        """
-            Control the end effector pose of the robot arm.
-            Args:
-                left_pose (KuavoPose): Pose of the robot left arm, xyz and quat.
-                right_pose (KuavoPose): Pose of the robot right arm, xyz and quat.
-                frame (KuavoManipulationMpcFrame): Frame of the robot end effector pose.
-            Returns:
-                bool: True if the control was successful, False otherwise.
-        """
-        return self._kuavo_core.control_robot_end_effector_pose(left_pose, right_pose, frame)
+        return self._kuavo_core.control_robot_arm_target_poses(times=times, joint_q=q_degs)
 
     def set_fixed_arm_mode(self) -> bool:
         """
@@ -105,30 +90,6 @@ class KuavoRobotArm:
         """
         return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl)
 
-    def set_manipulation_mpc_mode(self, ctrl_mode: KuavoManipulationMpcCtrlMode) -> bool:
-        """
-        Set the manipulation mpc mode.
-        Returns:
-            bool: True if the manipulation mpc mode is set successfully, False otherwise.
-        """
-        return self._kuavo_core.change_manipulation_mpc_ctrl_mode(ctrl_mode)
-    
-    def set_manipulation_mpc_control_flow(self, control_flow: KuavoManipulationMpcControlFlow) -> bool:
-        """
-        Set the manipulation mpc control flow.
-        Returns:
-            bool: True if the manipulation mpc control flow is set successfully, False otherwise.
-        """
-        return self._kuavo_core.change_manipulation_mpc_control_flow(control_flow)
-    
-    def set_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame) -> bool:
-        """
-        Set the manipulation mpc frame.
-        Returns:
-            bool: True if the manipulation mpc frame is set successfully, False otherwise.
-        """
-        return self._kuavo_core.change_manipulation_mpc_frame(frame)
-    
     """ Arm Forward kinematics && Arm Inverse kinematics """
     def arm_ik(self, 
                left_pose: KuavoPose, 
@@ -183,10 +144,3 @@ class KuavoRobotArm:
         if result is None:
             return None, None
         return result
-
-# if __name__ == "__main__":
-#     arm = KuavoRobotArm()
-#     arm.set_manipulation_mpc_mode(KuavoManipulationMpcCtrlMode.ArmOnly)
-#     arm.set_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.DirectToWbc)
-#     arm.set_manipulation_mpc_frame(KuavoManipulationMpcFrame.WorldFrame)
-#     arm.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)

kuavo_humanoid_sdk/kuavo/robot_head.py

@@ -27,13 +27,3 @@ class KuavoRobotHead:
             SDKLogger.warn(f"[Robot] pitch {pitch} exceeds limit [-{math.pi/7.2:.3f}, {math.pi/7.2:.3f}] radians (-25 to 25 degrees), will be limited")
         limited_pitch = min(math.pi/7.2, max(-math.pi/7.2, pitch))
         return self._kuavo_core.control_robot_head(yaw=limited_yaw, pitch=limited_pitch)
-
-    def enable_head_tracking(self, target_id: int)->bool:
-        """Enable the head tracking.
-        """
-        return self._kuavo_core.enable_head_tracking(target_id)
-    
-    def disable_head_tracking(self)->bool:
-        """Disable the head tracking.
-        """
-        return self._kuavo_core.disable_head_tracking()