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

kuavo_humanoid_sdk/__init__.py

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

kuavo_humanoid_sdk/interfaces/data_types.py

@@ -60,6 +60,50 @@ class KuavoArmCtrlMode(Enum):
     AutoSwing = 1 
     ExternalControl = 2
 
+class KuavoManipulationMpcFrame(Enum):
+    """Enum class representing the manipulation mpc frame for the Kuavo robot end effector.
+
+    Attributes:
+        KeepCurrentFrame: Keep the current frame (value: 0)
+        WorldFrame: World frame (value: 1)
+        LocalFrame: Local frame (value: 2)
+        VRFrame: VR frame (value: 3)
+        ManipulationWorldFrame: Manipulation world frame (value: 4)
+    """
+    KeepCurrentFrame = 0
+    WorldFrame = 1
+    LocalFrame = 2
+    VRFrame = 3
+    ManipulationWorldFrame = 4
+    ERROR = -1
+
+class KuavoManipulationMpcCtrlMode(Enum):
+    """Enum class representing the control mode for the Kuavo robot manipulation MPC.
+
+    Attributes:
+        NoControl: No control (value: 0)
+        ArmOnly: Only control the arm (value: 1)
+        BaseOnly: Only control the base (value: 2)
+        BaseArm: Control both the base and the arm (value: 3)
+        ERROR: Error state (value: -1)
+    """
+    NoControl = 0
+    ArmOnly = 1
+    BaseOnly = 2
+    BaseArm = 3
+    ERROR = -1
+
+class KuavoManipulationMpcControlFlow(Enum):
+    """Enum class representing the control data flow for the Kuavo robot manipulation.
+
+    Attributes:
+        ThroughFullBodyMpc: Control data flows through full-body MPC before entering WBC (value: 0)
+        DirectToWbc: Control data flows directly to WBC without full-body MPC (value: 1)
+        Error: Invalid control path (value: -1)
+    """
+    ThroughFullBodyMpc = 0
+    DirectToWbc = 1
+    Error = -1
 
 @dataclass
 class EndEffectorState:

kuavo_humanoid_sdk/kuavo/core/core.py

@@ -24,9 +24,9 @@ import numpy as np
 from typing import Tuple
 from transitions import Machine, State
 
-from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
 from kuavo_humanoid_sdk.kuavo.core.ros.control import KuavoRobotControl, KuavoRobotControlWebsocket
-from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore, KuavoRobotVisionCoreWebsocket
+from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore
 from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCore
 from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
 from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param
@@ -37,15 +37,19 @@ ROBOT_STATES = [
     State(name='stance', on_enter=['_on_enter_stance']),
     State(name='walk', on_enter=['_on_enter_walk']), 
     State(name='trot', on_enter=['_on_enter_trot']),
-    State(name='custom_gait', on_enter=['_on_enter_custom_gait'])
+    State(name='custom_gait', on_enter=['_on_enter_custom_gait']),
+    State(name='command_pose_world', on_enter=['_on_enter_command_pose_world']),
+    State(name='command_pose', on_enter=['_on_enter_command_pose']),
 ]
 
 # Define state transitions
 ROBOT_TRANSITIONS = [
-    {'trigger': 'to_stance', 'source': ['walk', 'trot', 'custom_gait'], 'dest': 'stance'},
+    {'trigger': 'to_stance', 'source': ['walk', 'trot', 'custom_gait', 'command_pose_world', 'command_pose'], 'dest': 'stance'},
     {'trigger': 'to_walk', 'source': ['stance', 'trot', 'custom_gait'], 'dest': 'walk'},
     {'trigger': 'to_trot', 'source': ['stance', 'walk', 'custom_gait'], 'dest': 'trot'},
     {'trigger': 'to_custom_gait', 'source': ['stance', 'custom_gait'], 'dest': 'custom_gait'},
+    {'trigger': 'to_command_pose_world', 'source': ['stance', 'command_pose_world'], 'dest': 'command_pose_world'},
+    {'trigger': 'to_command_pose', 'source': ['stance', 'command_pose'], 'dest': 'command_pose'},
 ]
 
 class KuavoRobotCore:
@@ -69,14 +73,19 @@ class KuavoRobotCore:
             if GlobalConfig.use_websocket:
                 self._control = KuavoRobotControlWebsocket()
                 self._rb_state = KuavoRobotStateCoreWebsocket()
-                self._robot_vision = KuavoRobotVisionCoreWebsocket()
             else:
                 self._control = KuavoRobotControl()
                 self._rb_state = KuavoRobotStateCore()
-                # robot vision
-                self._robot_vision = KuavoRobotVisionCore()
-                # robot ros tf
-                self._robot_tf_tool = KuavoRobotToolsCore()
+    
+            # manipulation mpc
+            self._manipulation_mpc_frame = KuavoManipulationMpcFrame.KeepCurrentFrame
+            self._manipulation_mpc_ctrl_mode = KuavoManipulationMpcCtrlMode.NoControl
+            self._manipulation_mpc_control_flow = KuavoManipulationMpcControlFlow.ThroughFullBodyMpc
+
+            # robot vision
+            self._robot_vision = KuavoRobotVisionCore()
+            # robot ros tf
+            self._robot_tf_tool = KuavoRobotToolsCore()
                 
             self._arm_ctrl_mode = KuavoArmCtrlMode.AutoSwing
             
@@ -105,6 +114,21 @@ class KuavoRobotCore:
             if arm_ctrl_mode is not None:
                 self._arm_ctrl_mode = arm_ctrl_mode
                 SDKLogger.debug(f"[Core] initialize arm control mode: {arm_ctrl_mode}")
+            
+            # init manipulation mpc
+            manipulation_mpc_frame = self._rb_state.manipulation_mpc_frame
+            if manipulation_mpc_frame is not None:
+                self._manipulation_mpc_frame = manipulation_mpc_frame
+                SDKLogger.debug(f"[Core] initialize manipulation mpc frame: {manipulation_mpc_frame}")
+            manipulation_mpc_ctrl_mode = self._rb_state.manipulation_mpc_ctrl_mode
+            if manipulation_mpc_ctrl_mode is not None:
+                self._manipulation_mpc_ctrl_mode = manipulation_mpc_ctrl_mode
+                SDKLogger.debug(f"[Core] initialize manipulation mpc ctrl mode: {manipulation_mpc_ctrl_mode}")
+            manipulation_mpc_control_flow = self._rb_state.manipulation_mpc_control_flow
+            if manipulation_mpc_control_flow is not None:
+                self._manipulation_mpc_control_flow = manipulation_mpc_control_flow
+                SDKLogger.debug(f"[Core] initialize manipulation mpc control flow: {manipulation_mpc_control_flow}")
+                
         except Exception as e:
             raise RuntimeError(f"[Core] initialize failed: \n"
                              f"{e}, please check the robot is launched, "
@@ -166,6 +190,21 @@ class KuavoRobotCore:
             SDKLogger.debug(f"[Core] [StateMachine] State unchanged: already in custom_gait state")
             return
         SDKLogger.debug(f"[Core] [StateMachine] Entering custom_gait state, from {previous_state}")
+    
+    def _on_enter_command_pose_world(self, event):
+        previous_state = event.transition.source
+        if self.state  == previous_state:
+            SDKLogger.debug(f"[Core] [StateMachine] State unchanged: already in command_pose_world state")
+            return
+        SDKLogger.debug(f"[Core] [StateMachine] Entering command_pose_world state, from {previous_state}")
+
+    def _on_enter_command_pose(self, event):
+        previous_state = event.transition.source
+        if self.state  == previous_state:
+            SDKLogger.debug(f"[Core] [StateMachine] State unchanged: already in command_pose state")
+            return
+        SDKLogger.debug(f"[Core] [StateMachine] Entering command_pose state, from {previous_state}")
+        
     """ -------------------------------------------------------------"""
 
     """ --------------------------- Control -------------------------"""
@@ -301,6 +340,54 @@ class KuavoRobotCore:
 
         return True
 
+    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
+        
+        Arguments:
+            - target_pose_x: x position (meters)
+            - target_pose_y: y position (meters)
+            - target_pose_z: z position (meters)
+            - target_pose_yaw: yaw angle (radians)
+        
+        Returns:
+            bool: True if command was sent successfully, False otherwise
+            
+        Raises:
+            RuntimeError: If robot is not in stance state
+        """
+        if self.state != 'stance':
+            raise RuntimeError(f"[Core] control_command_pose failed: robot must be in stance state, current state: {self.state}")
+        
+        # Add any parameter validation if needed
+        # e.g., limit ranges for safety
+        self.to_command_pose()
+        return self._control.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
+        
+        Arguments:
+            - target_pose_x: x position (meters)
+            - target_pose_y: y position (meters)
+            - target_pose_z: z position (meters)
+            - target_pose_yaw: yaw angle (radians)
+        
+        Returns:
+            bool: True if command was sent successfully, False otherwise
+            
+        Raises:
+            RuntimeError: If robot is not in stance state
+        """
+        # if self.state != 'stance':
+        #     raise RuntimeError(f"[Core] control_command_pose_world failed: robot must be in stance state, current state: {self.state}")
+        
+        # Add any parameter validation if needed
+        # e.g., limit ranges for safety
+        self.to_command_pose_world()
+        return self._control.control_command_pose_world(target_pose_x, target_pose_y, target_pose_z, target_pose_yaw)
+    
     def execute_gesture(self, gestures:list)->bool:
         return self._control.execute_gesture(gestures)
     
@@ -323,30 +410,111 @@ class KuavoRobotCore:
         pitch_deg = pitch * 180 / math.pi
         return self._control.control_robot_head(yaw_deg, pitch_deg)
     
-    def control_robot_arm_traj(self, joint_data:list)->bool:
+    def enable_head_tracking(self, target_id: int)->bool:
+        return self._control.enable_head_tracking(target_id)
+    
+    def disable_head_tracking(self)->bool:
+        return self._control.disable_head_tracking()
+    
+    def control_robot_arm_joint_positions(self, joint_data:list)->bool:
         if self.state != 'stance':
-            raise RuntimeError(f"[Core] control_robot_arm_traj failed: robot must be in stance state, current state: {self.state}")
+            raise RuntimeError(f"[Core] control_robot_arm_joint_positions failed: robot must be in stance state, current state: {self.state}")
         
         # change to external control mode  
         if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
-            SDKLogger.debug("[Core] control_robot_arm_traj, current arm mode != ExternalControl, change it.")
+            SDKLogger.debug("[Core] control_robot_arm_joint_positions, current arm mode != ExternalControl, change it.")
             if not self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl):
-                SDKLogger.warn("[Core] control_robot_arm_traj failed, change robot arm ctrl mode failed!")
+                SDKLogger.warn("[Core] control_robot_arm_joint_positions failed, change robot arm ctrl mode failed!")
                 return False
-        return self._control.control_robot_arm_traj(joint_data)
+        return self._control.control_robot_arm_joint_positions(joint_data)
     
-    def control_robot_arm_target_poses(self, times:list, joint_q:list)->bool:
+    def control_robot_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
         if self.state != 'stance':
-            raise RuntimeError("[Core] control_robot_arm_target_poses failed: robot must be in stance state")
+            raise RuntimeError("[Core] control_robot_arm_joint_trajectory failed: robot must be in stance state")
         
         if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
-            SDKLogger.debug("[Core] control_robot_arm_target_poses, current arm mode != ExternalControl, change it.")
+            SDKLogger.debug("[Core] control_robot_arm_joint_trajectory, current arm mode != ExternalControl, change it.")
             if not self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl):
-                SDKLogger.warn("[Core] control_robot_arm_target_poses failed, change robot arm ctrl mode failed!")
+                SDKLogger.warn("[Core] control_robot_arm_joint_trajectory failed, change robot arm ctrl mode failed!")
                 return False
             
-        return self._control.control_robot_arm_target_poses(times, joint_q)
+        return self._control.control_robot_arm_joint_trajectory(times, joint_q)
+    
+    def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:        
+        if self._arm_ctrl_mode != KuavoArmCtrlMode.ExternalControl:
+            SDKLogger.debug("[Core] control_robot_end_effector_pose, current arm mode != ExternalControl, change it.")
+            if not self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl):
+                SDKLogger.warn("[Core] control_robot_end_effector_pose failed, change robot arm ctrl mode failed!")
+                return False
+
+        if self._manipulation_mpc_ctrl_mode == KuavoManipulationMpcCtrlMode.NoControl:
+            SDKLogger.debug("[Core] control_robot_end_effector_pose, manipulation mpc ctrl mode is NoControl, change it.")
+            if not self.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.ArmOnly):
+                SDKLogger.warn("[Core] control_robot_end_effector_pose failed, change manipulation mpc ctrl mode failed!")
+                return False
+        
+        return self._control.control_robot_end_effector_pose(left_pose, right_pose, frame)
 
+    def change_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame)->bool:
+        timeout = 1.0
+        count = 0
+        while self._rb_state.manipulation_mpc_frame != frame:
+            SDKLogger.debug(f"[Core] Change manipulation mpc frame from {self._rb_state.manipulation_mpc_frame} to {frame}, retry: {count}")
+            self._control.change_manipulation_mpc_frame(frame)
+            if self._rb_state.manipulation_mpc_frame == frame:
+                break
+            if timeout <= 0:
+                SDKLogger.warn("[Core] Change manipulation mpc frame timeout!")
+                return False
+            timeout -= 0.1
+            time.sleep(0.1)
+            count += 1
+        if not hasattr(self, '_manipulation_mpc_frame_lock'):
+            self._manipulation_mpc_frame_lock = threading.Lock()
+        with self._manipulation_mpc_frame_lock:
+            self._manipulation_mpc_frame = frame
+        return True
+    
+    def change_manipulation_mpc_ctrl_mode(self, control_mode: KuavoManipulationMpcCtrlMode)->bool:
+        timeout = 1.0
+        count = 0
+        while self._rb_state.manipulation_mpc_ctrl_mode != control_mode:
+            SDKLogger.debug(f"[Core] Change manipulation mpc ctrl mode from {self._rb_state.manipulation_mpc_ctrl_mode} to {control_mode}, retry: {count}")
+            self._control.change_manipulation_mpc_ctrl_mode(control_mode)
+            if self._rb_state.manipulation_mpc_ctrl_mode == control_mode:
+                break
+            if timeout <= 0:
+                SDKLogger.warn("[Core] Change manipulation mpc ctrl mode timeout!")
+                return False
+            timeout -= 0.1
+            time.sleep(0.1)
+            count += 1
+        if not hasattr(self, '_manipulation_mpc_ctrl_mode_lock'):
+            self._manipulation_mpc_ctrl_mode_lock = threading.Lock()
+        with self._manipulation_mpc_ctrl_mode_lock:
+            self._manipulation_mpc_ctrl_mode = control_mode
+        return True
+    
+    def change_manipulation_mpc_control_flow(self, control_flow: KuavoManipulationMpcControlFlow)->bool:
+        timeout = 1.0
+        count = 0
+        while self._rb_state.manipulation_mpc_control_flow != control_flow:
+            SDKLogger.debug(f"[Core] Change manipulation mpc control flow from {self._rb_state.manipulation_mpc_control_flow} to {control_flow}, retry: {count}")
+            self._control.change_manipulation_mpc_control_flow(control_flow)
+            if self._rb_state.manipulation_mpc_control_flow == control_flow:
+                break
+            if timeout <= 0:
+                SDKLogger.warn("[Core] Change manipulation mpc control flow timeout!")
+                return False
+            timeout -= 0.1
+            time.sleep(0.1)
+            count += 1
+        if not hasattr(self, '_manipulation_mpc_control_flow_lock'):
+            self._manipulation_mpc_control_flow_lock = threading.Lock()
+        with self._manipulation_mpc_control_flow_lock:
+            self._manipulation_mpc_control_flow = control_flow
+        return True
+    
     def change_robot_arm_ctrl_mode(self, mode:KuavoArmCtrlMode)->bool:
         timeout = 1.0
         count = 0
@@ -376,12 +544,25 @@ class KuavoRobotCore:
             return
         
         # init_pos = [0.0]*14
-        # if not self.control_robot_arm_target_poses([1.5], [init_pos]):
+        # if not self.control_robot_arm_joint_trajectory([1.5], [init_pos]):
         #     SDKLogger.warn("[Core] robot arm reset failed, control robot arm traj failed!")
         #     return False
         
         return self.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.AutoSwing)
         
+    def robot_manipulation_mpc_reset(self)->bool:
+        if self._manipulation_mpc_ctrl_mode != KuavoManipulationMpcCtrlMode.NoControl:
+            SDKLogger.debug("[Core] robot manipulation mpc reset, current manipulation mpc ctrl mode != NoControl, change it.")
+            if not self.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.NoControl):
+                SDKLogger.warn("[Core] robot manipulation mpc reset failed, change manipulation mpc ctrl mode failed!")
+                return False
+        if self._manipulation_mpc_control_flow != KuavoManipulationMpcControlFlow.ThroughFullBodyMpc:
+            SDKLogger.debug("[Core] robot manipulation mpc reset, current manipulation mpc control flow != ThroughFullBodyMpc, change it.")
+            if not self.change_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.ThroughFullBodyMpc):
+                SDKLogger.warn("[Core] robot manipulation mpc reset failed, change manipulation mpc control flow failed!")
+                return False
+        return True
+        
     """ ------------------------------------------------------------------------"""
     """ Arm Forward kinematics && Arm Inverse kinematics """
     def arm_ik(self, 
@@ -406,3 +587,28 @@ class KuavoRobotCore:
                 SDKLogger.debug(f"[Core] Received gait change notification: {gait_name} at time {current_time}")
                 # Call the transition method if it exists
                 getattr(self, to_method)()
+
+
+if __name__ == "__main__":
+    DEBUG_MODE = 0
+    core = KuavoRobotCore()
+    core.initialize()
+
+    if DEBUG_MODE == 0:
+        time.sleep(1.0)
+        core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl)
+        core.change_manipulation_mpc_frame(KuavoManipulationMpcFrame.VRFrame)
+        core.change_manipulation_mpc_ctrl_mode(KuavoManipulationMpcCtrlMode.ArmOnly)
+        core.change_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.DirectToWbc)
+        core.robot_manipulation_mpc_reset()
+    elif DEBUG_MODE == 1:
+        core.to_stance()
+        print("state now is to_stance:", core.state)
+        core.control_command_pose_world(0.0, 1.0, 0.0, 1.57)
+        print("state now is control_command_pose_world:", core.state)
+    elif DEBUG_MODE == 2:
+        core.to_trot()
+        print("state now is to_trot:", core.state)
+        time.sleep(3.0)
+        core.to_stance()
+        print("state now is to_stance:", core.state)