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

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())

kuavo_humanoid_sdk/kuavo_strategy/__init__.py

@@ -0,0 +1,2 @@
+from .kuavo_strategy import *
+from .grasp_box.grasp_box_strategy import *