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

kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -1,5 +1,4 @@
 import roslibpy
-import copy
 import time
 from typing import Tuple
 
@@ -14,12 +13,14 @@ try:
 except:
     pass
 
-from kuavo_humanoid_sdk.interfaces.data_types import (KuavoImuData, KuavoJointData, KuavoOdometry, 
-                                                      KuavoArmCtrlMode, EndEffectorState, KuavoDexHandTouchState)
+from kuavo_humanoid_sdk.interfaces.data_types import (KuavoImuData, KuavoJointData, KuavoOdometry, KuavoManipulationMpcFrame, 
+                                                      KuavoArmCtrlMode, EndEffectorState, KuavoDexHandTouchState,
+                                                      KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow)
 from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
 from kuavo_humanoid_sdk.common.logger import SDKLogger
 from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
-
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import (changeArmCtrlMode, changeArmCtrlModeRequest,setMmCtrlFrame, setMmCtrlFrameRequest,
+                                                   changeTorsoCtrlMode, changeTorsoCtrlModeRequest)
 from collections import deque
 from typing import Tuple, Optional
 
@@ -157,6 +158,11 @@ class KuavoRobotStateCore:
             self._arm_ctrl_mode = self._srv_get_arm_ctrl_mode()
             self._initialized = True
 
+            # 获取manipulation mpc 相关参数
+            self._manipulation_mpc_frame = self._srv_get_manipulation_mpc_frame()
+            self._manipulation_mpc_ctrl_mode = self._srv_get_manipulation_mpc_ctrl_mode()
+            self._manipulation_mpc_control_flow = self._srv_get_manipulation_mpc_control_flow()
+
     @property
     def com_height(self)->float:
         # odom.position.z - terrain_height = com_height
@@ -181,6 +187,27 @@ class KuavoRobotStateCore:
             self._arm_ctrl_mode = mode
         return self._arm_ctrl_mode
     
+    @property
+    def manipulation_mpc_ctrl_mode(self)->KuavoManipulationMpcCtrlMode:
+        mode = self._srv_get_manipulation_mpc_ctrl_mode()
+        if mode is not None:
+            self._manipulation_mpc_ctrl_mode = mode
+        return self._manipulation_mpc_ctrl_mode
+    
+    @property
+    def manipulation_mpc_frame(self)->KuavoManipulationMpcFrame:
+        frame = self._srv_get_manipulation_mpc_frame()
+        if frame is not None:
+            self._manipulation_mpc_frame = frame
+        return self._manipulation_mpc_frame
+    
+    @property
+    def manipulation_mpc_control_flow(self)->KuavoManipulationMpcControlFlow:
+        flow = self._srv_get_manipulation_mpc_control_flow()
+        if flow is not None:
+            self._manipulation_mpc_control_flow = flow
+        return self._manipulation_mpc_control_flow
+    
     @property
     def eef_state(self)->Tuple[EndEffectorState, EndEffectorState]:
         return self._eef_state
@@ -387,6 +414,194 @@ class KuavoRobotStateCore:
             SDKLogger.error(f"Service call failed: {e}")
         return None
 
+    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
+        try:
+            service_name = '/mobile_manipulator_get_mpc_control_mode'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
+            
+            req = changeTorsoCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
+                return KuavoManipulationMpcCtrlMode.ERROR
+            return KuavoManipulationMpcCtrlMode(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
+        return KuavoManipulationMpcCtrlMode.ERROR
+
+    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
+        try:
+            service_name = '/get_mm_ctrl_frame'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
+            
+            req = setMmCtrlFrameRequest()
+            
+            resp = get_frame_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
+                return KuavoManipulationMpcFrame.ERROR
+            return KuavoManipulationMpcFrame(resp.currentFrame)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
+        return KuavoManipulationMpcFrame.ERROR
+
+    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
+        try:
+            service_name = '/get_mm_wbc_arm_trajectory_control'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
+            
+            req = changeArmCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
+                return KuavoManipulationMpcControlFlow.Error
+            return KuavoManipulationMpcControlFlow(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
+        return KuavoManipulationMpcControlFlow.Error
+
+    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
+        try:
+            service_name = '/mobile_manipulator_get_mpc_control_mode'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
+            
+            req = changeTorsoCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
+                return KuavoManipulationMpcCtrlMode.ERROR
+            return KuavoManipulationMpcCtrlMode(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
+        return KuavoManipulationMpcCtrlMode.ERROR
+
+    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
+        try:
+            service_name = '/get_mm_ctrl_frame'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
+            
+            req = setMmCtrlFrameRequest()
+            
+            resp = get_frame_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
+                return KuavoManipulationMpcFrame.ERROR
+            return KuavoManipulationMpcFrame(resp.currentFrame)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
+        return KuavoManipulationMpcFrame.ERROR
+
+    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
+        try:
+            service_name = '/get_mm_wbc_arm_trajectory_control'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
+            
+            req = changeArmCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
+                return KuavoManipulationMpcControlFlow.Error
+            return KuavoManipulationMpcControlFlow(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
+        return KuavoManipulationMpcControlFlow.Error
+
+    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
+        try:
+            service_name = '/mobile_manipulator_get_mpc_control_mode'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
+            
+            req = changeTorsoCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
+                return KuavoManipulationMpcCtrlMode.ERROR
+            return KuavoManipulationMpcCtrlMode(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
+        return KuavoManipulationMpcCtrlMode.ERROR
+
+    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
+        try:
+            service_name = '/get_mm_ctrl_frame'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
+            
+            req = setMmCtrlFrameRequest()
+            
+            resp = get_frame_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
+                return KuavoManipulationMpcFrame.ERROR
+            return KuavoManipulationMpcFrame(resp.currentFrame)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
+        return KuavoManipulationMpcFrame.ERROR
+
+    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
+        try:
+            service_name = '/get_mm_wbc_arm_trajectory_control'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
+            
+            req = changeArmCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
+                return KuavoManipulationMpcControlFlow.Error
+            return KuavoManipulationMpcControlFlow(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
+        return KuavoManipulationMpcControlFlow.Error
 class KuavoRobotStateCoreWebsocket:
     _instance = None
 
@@ -691,4 +906,12 @@ class KuavoRobotStateCoreWebsocket:
             return None
         except Exception as e:
             SDKLogger.error(f"Service call failed: {e}")
-        return None
+        return None
+
+
+if __name__ == "__main__":
+    state = KuavoRobotStateCore()
+    print(state.manipulation_mpc_frame)
+    print(state.manipulation_mpc_control_flow)
+    print(state.manipulation_mpc_ctrl_mode)
+    print(state.arm_control_mode)

kuavo_humanoid_sdk/kuavo/core/ros/vision.py

@@ -34,16 +34,16 @@ class KuavoRobotVisionCore:
     def __init__(self):
         """Initializes vision system components including TF and AprilTag subscribers."""
         if not hasattr(self, '_initialized'):
+            # Initialize TF components
             self.tf_buffer = tf2_ros.Buffer()
             self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
             self.tf_broadcaster = tf2_ros.TransformBroadcaster()
-            self._apriltag_data_camera_sub = rospy.Subscriber('/tag_detections', AprilTagDetectionArray, self._apriltag_data_callback_camera)
-            self._apriltag_data_base_sub = rospy.Subscriber('/robot_tag_info', AprilTagDetectionArray, self._apriltag_data_callback_base)
-
+            
             # 添加TF2监听器
             self._tf_buffer = tf2_ros.Buffer()
             self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
 
+            # FIRST: Initialize data structures before creating subscribers
             """ data """
             self._apriltag_data_from_camera = AprilTagData(
                 id = [],
@@ -62,6 +62,13 @@ class KuavoRobotVisionCore:
                 size = [],
                 pose = []
             )
+            
+            # THEN: Create subscribers after all data structures are initialized
+            self._apriltag_data_camera_sub = rospy.Subscriber('/tag_detections', AprilTagDetectionArray, self._apriltag_data_callback_camera)
+            self._apriltag_data_base_sub = rospy.Subscriber('/robot_tag_info', AprilTagDetectionArray, self._apriltag_data_callback_base)
+            
+            # Mark as initialized
+            self._initialized = True
 
     def _apriltag_data_callback_camera(self, data):
         """Callback for processing AprilTag detections from camera.
@@ -147,7 +154,7 @@ class KuavoRobotVisionCore:
         
         # 如果base数据为空，则不处理
         if not self._apriltag_data_from_base.id:
-            SDKLogger.warn("No base tag data, skip transform")
+            # SDKLogger.warn("No base tag data, skip transform")
             return
         
         try:
@@ -246,7 +253,7 @@ class KuavoRobotVisionCore:
         }
         
         if data_source not in data_map:
-            SDKLogger.error(f"Invalid data source: {data_source}, must be one of {list(data_map.keys())}")
+            # SDKLogger.error(f"Invalid data source: {data_source}, must be one of {list(data_map.keys())}")
             return None
         
         data = data_map[data_source]
@@ -255,7 +262,7 @@ class KuavoRobotVisionCore:
         indices = [i for i, tag_id in enumerate(data.id) if tag_id == target_id]
         
         if not indices:
-            SDKLogger.debug(f"No data found for tag ID {target_id} in {data_source} source")
+            # SDKLogger.debug(f"No data found for tag ID {target_id} in {data_source} source")
             return None
         
         return {

kuavo_humanoid_sdk/kuavo/robot.py

@@ -5,7 +5,7 @@ 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
+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
@@ -219,6 +219,46 @@ 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.
@@ -232,6 +272,16 @@ 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.
@@ -240,8 +290,16 @@ 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.
         
-    def control_arm_position(self, joint_positions:list)->bool:
+        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:
         """Control the position of the arm.
         
         Args:
@@ -259,9 +317,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_position(joint_positions)
+        return self._robot_arm.control_arm_joint_positions(joint_positions)
     
-    def control_arm_target_poses(self, times:list, q_frames:list)->bool:
+    def control_arm_joint_trajectory(self, times:list, q_frames:list)->bool:
         """Control the target poses of the robot arm.
         
         Args:
@@ -281,7 +339,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_target_poses(times, q_frames)
+        return self._robot_arm.control_arm_joint_trajectory(times, q_frames)
 
     def set_fixed_arm_mode(self) -> bool:
         """Freezes the robot arm.
@@ -307,6 +365,30 @@ 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, 
@@ -354,4 +436,24 @@ class KuavoRobot(RobotBase):
         Warning:
             This function requires initializing the SDK with the :attr:`KuavoSDK.Options.WithIK`.        
         """
-        return self._robot_arm.arm_fk(q)
+        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)

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
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
 from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
 from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
 
@@ -14,8 +14,11 @@ 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_position(self, joint_position:list)->bool:
+    def control_arm_joint_positions(self, joint_position:list)->bool:
         """
             Control the position of the robot arm joint.
             Args:
@@ -35,9 +38,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_traj(joint_data=joint_position)
+        return self._kuavo_core.control_robot_arm_joint_positions(joint_data=joint_position)
 
-    def control_arm_target_poses(self, times:list, q_frames:list)->bool:
+    def control_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
         """
             Control the target poses of the robot arm.
             Args:
@@ -51,12 +54,12 @@ class KuavoRobotArm:
             Returns:
                 bool: True if the control was successful, False otherwise.    
         """
-        if len(times) != len(q_frames):
+        if len(times) != len(joint_q):
             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 q_frames:
+        for q in joint_q:
             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:
@@ -64,7 +67,19 @@ 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_target_poses(times=times, joint_q=q_degs)
+        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)
 
     def set_fixed_arm_mode(self) -> bool:
         """
@@ -90,6 +105,30 @@ 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, 
@@ -144,3 +183,10 @@ 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,3 +27,13 @@ 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()

kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -3,7 +3,8 @@
 import time
 from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.data_types import (
-    KuavoImuData, KuavoJointData, KuavoOdometry, KuavoArmCtrlMode,EndEffectorState)
+    KuavoImuData, KuavoJointData, KuavoOdometry, KuavoArmCtrlMode,EndEffectorState, 
+    KuavoManipulationMpcControlFlow, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcFrame)
 from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
 from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
@@ -147,6 +148,31 @@ class KuavoRobotState:
         """
         return self._rs_core.arm_control_mode
     
+    def manipulation_mpc_ctrl_mode(self) -> KuavoManipulationMpcCtrlMode:
+        """Get the current control mode of the robot manipulation MPC.
+
+        Returns:
+            KuavoManipulationMpcCtrlMode: Current manipulation MPC control mode.
+
+        """
+        return self._rs_core.manipulation_mpc_ctrl_mode
+    
+    def manipulation_mpc_control_flow(self) -> KuavoManipulationMpcControlFlow:
+        """Get the current control flow of the robot manipulation.
+
+        Returns:
+            KuavoManipulationMpcControlFlow: Current manipulation control flow.
+        """
+        return self._rs_core.manipulation_mpc_control_flow
+    
+    def manipulation_mpc_frame(self) -> KuavoManipulationMpcFrame:
+        """Get the current frame of the robot manipulation MPC.
+
+        Returns:
+            KuavoManipulationMpcFrame: Current manipulation MPC frame.
+        """
+        return self._rs_core.manipulation_mpc_frame
+    
     def head_joint_state(self) -> KuavoJointData:
         """Get the current state of the robot head joints.
 
@@ -270,4 +296,11 @@ class KuavoRobotState:
         while not self._rs_core.is_gait('custom_gait') and wait_time < timeout:
             time.sleep(0.1)
             wait_time += 0.1
-        return self._rs_core.is_gait('custom_gait')
+        return self._rs_core.is_gait('custom_gait')
+    
+# if __name__ == "__main__":
+#     state = KuavoRobotState()
+#     print(state.manipulation_mpc_frame())
+#     print(state.manipulation_mpc_control_flow())
+#     print(state.manipulation_mpc_ctrl_mode())
+#     print(state.arm_control_mode())