kuavo-humanoid-sdk 1.2.1b3302__20250916170841-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -0,0 +1,310 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import time
+import copy
+from typing import Tuple
+from kuavo_humanoid_sdk.interfaces.data_types import (
+    KuavoImuData, KuavoJointData, KuavoOdometry, KuavoArmCtrlMode,EndEffectorState, 
+    KuavoManipulationMpcControlFlow, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcFrame)
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+
+class KuavoRobotState:
+    def __init__(self, robot_type: str = "kuavo"):
+        self._rs_core = KuavoRobotStateCore()
+
+    @property
+    def imu_data(self) -> KuavoImuData:
+        """获取 Kuavo 机器人IMU数据。
+
+        获取机器人当前的 IMU 传感器数据，包括陀螺仪、加速度计、自由加速度和方向四元数测量值。
+
+        Returns:
+            :class:`KuavoImuData`: IMU数据包含:
+                * gyro (:obj:`tuple` of :obj:`float`): 陀螺仪测量值 (x, y, z)，单位rad/s
+                * acc (:obj:`tuple` of :obj:`float`): 加速度计测量值 (x, y, z)，单位m/s^2
+                * free_acc (:obj:`tuple` of :obj:`float`): 自由加速度 (x, y, z)，单位m/s^2
+                * quat (:obj:`tuple` of :obj:`float`): 方向四元数 (x, y, z, w)
+        """
+        return self._rs_core.imu_data
+    
+    @property
+    def joint_state(self) -> KuavoJointData:
+        """获取 Kuavo 机器人关节数据。
+
+        获取机器人关节数据，包括关节位置、速度、扭矩和加速度。
+
+        数据包括:
+            - 关节位置(角度)，单位为弧度
+            - 关节速度，单位为弧度/秒
+            - 关节扭矩/力矩，单位为牛顿米、安培
+            - 关节加速度
+
+        Returns:
+            KuavoJointData: 包含以下关节状态数据的字典:
+                * position (list[float]): 关节位置，长度 = joint_dof(28)
+                * velocity (list[float]): 关节速度，长度 = joint_dof(28)
+                * torque (list[float]): 关节扭矩，长度 = joint_dof(28)
+                * acceleration (list[float]): 关节加速度，长度 = joint_dof(28)
+        """
+        return self._rs_core.joint_data
+    
+    @property
+    def com_height(self)->float:
+        """获取机器人实时的质心高度。
+
+        Returns:
+            float: 机器人质心高度，单位为米。
+            
+        Note:
+            如果需要获取机器人初始化站立时的质心高度，请使用 :attr:`KuavoRobotInfo.init_stand_height` 属性。
+        """
+        return self._rs_core.com_height
+    
+    @property
+    def odometry(self) -> KuavoOdometry:
+        """获取 Kuavo 机器人里程计数据。
+
+        获取机器人当前的里程计数据，包括位置、方向、线速度和角速度测量值。
+
+        Returns:
+            KuavoOdometry: 包含以下里程计数据的字典:
+                * position (tuple): 位置 (x, y, z)，单位为米
+                * orientation (tuple): 方向四元数 (x, y, z, w)
+                * linear (tuple): 线速度 (x, y, z)，单位为m/s
+                * angular (tuple): 角速度 (x, y, z)，单位为rad/s
+        """
+        return self._rs_core.odom_data   
+
+    
+    def robot_position(self) -> Tuple[float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的位置。
+
+        Returns:
+            Tuple[float, float, float]: 位置 (x, y, z)，单位为米。
+        """
+        return tuple(self._rs_core.odom_data.position)
+
+    def robot_orientation(self) -> Tuple[float, float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的方向。
+
+        Returns:
+            Tuple[float, float, float, float]: 方向四元数 (x, y, z, w)。
+        """
+        return tuple(self._rs_core.odom_data.orientation)
+
+    def linear_velocity(self) -> Tuple[float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的线速度。
+
+        Returns:
+            Tuple[float, float, float]: 线速度 (x, y, z)，单位为m/s。
+        """
+        return tuple(self._rs_core.odom_data.linear)
+    
+
+    def angular_velocity(self) -> Tuple[float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的角速度。
+
+        Returns:
+            Tuple[float, float, float]: 角速度 (x, y, z)。
+        """
+        return tuple(self._rs_core.odom_data.angular)
+
+    def arm_joint_state(self) -> KuavoJointData:
+        """获取 Kuavo 机器人手臂关节的当前状态。
+
+        获取 Kuavo 机器人手臂关节的当前状态，包括:
+            - 关节位置(角度)，单位为弧度
+            - 关节速度，单位为弧度/秒
+            - 关节扭矩/力矩，单位为牛顿米、安培
+            - 关节加速度
+
+        Returns:
+            KuavoJointData: 手臂关节数据包含:
+                * position: list[float] * arm_dof(14)
+                * velocity: list[float] * arm_dof(14)
+                * torque: list[float] * arm_dof(14)
+                * acceleration: list[float] * arm_dof(14)
+        """
+        # Get arm joint states from index 12 to 25 (14 arm joints)
+        arm_joint_indices = range(12, 12+14)
+        return KuavoJointData(
+            position=[self._rs_core.joint_data.position[i] for i in arm_joint_indices],
+            velocity=[self._rs_core.joint_data.velocity[i] for i in arm_joint_indices],
+            torque=[self._rs_core.joint_data.torque[i] for i in arm_joint_indices],
+            acceleration=[self._rs_core.joint_data.acceleration[i] for i in arm_joint_indices]
+        )
+
+    def arm_control_mode(self) -> KuavoArmCtrlMode:
+        """获取 Kuavo 机器人手臂的当前控制模式。
+
+        Returns:
+            KuavoArmCtrlMode: 当前手臂控制模式:
+                * ArmFixed: 0 - 机器人手臂处于固定位置。
+                * AutoSwing: 1 - 机器人手臂处于自动摆动模式。
+                * ExternalControl: 2 - 机器人手臂由外部控制。
+                * None - 机器人手臂处于未知状态。
+        """
+        return KuavoArmCtrlMode(self._rs_core.arm_control_mode)
+    
+    def manipulation_mpc_ctrl_mode(self) -> KuavoManipulationMpcCtrlMode:
+        """获取 Kuavo 机器人 Manipulation MPC 的当前控制模式。    
+
+        Returns:
+            KuavoManipulationMpcCtrlMode: 当前 Manipulation MPC 控制模式。
+        """
+        return self._rs_core.manipulation_mpc_ctrl_mode
+    
+    def manipulation_mpc_control_flow(self) -> KuavoManipulationMpcControlFlow:
+        """获取 Kuavo 机器人 Manipulation MPC 的当前控制流。
+
+        Returns:
+            KuavoManipulationMpcControlFlow: 当前 Manipulation MPC 控制流。
+        """
+        return self._rs_core.manipulation_mpc_control_flow
+    
+    def manipulation_mpc_frame(self) -> KuavoManipulationMpcFrame:
+        """获取机器人操作MPC的当前帧。
+
+        Returns:
+            KuavoManipulationMpcFrame: 末端执行器 Manipulation MPC 坐标系
+        """
+        return self._rs_core.manipulation_mpc_frame
+    
+    def pitch_limit_enabled(self) -> bool:
+        """获取机器人 basePitch 限制状态, 如果开启则返回True，否则返回False。
+
+        Returns:
+            bool: 如果机器人 basePitch 限制开启返回True，否则返回False。
+        """
+        return self._rs_core.pitch_limit_enabled
+
+    def head_joint_state(self) -> KuavoJointData:
+        """获取机器人头部关节的当前状态。
+
+        获取机器人头部关节的当前状态数据，包括位置、速度、扭矩和加速度值。
+
+        Returns:
+            KuavoJointData: 包含头部关节状态的数据结构:
+                * position (list[float]): 关节位置，单位为弧度，长度=head_dof(2)
+                * velocity (list[float]): 关节速度，单位为rad/s，长度=head_dof(2)
+                * torque (list[float]): 关节扭矩，单位为Nm，长度=head_dof(2)
+                * acceleration (list[float]): 关节加速度，单位为rad/s^2，长度=head_dof(2)
+                
+            关节顺序为 [偏航角, 俯仰角]。
+        """
+        # Get head joint states from last 2 indices
+        head_joint_indices = range(len(self._rs_core.joint_data.position)-2, len(self._rs_core.joint_data.position))
+        return KuavoJointData(
+            position=[self._rs_core.joint_data.position[i] for i in head_joint_indices],
+            velocity=[self._rs_core.joint_data.velocity[i] for i in head_joint_indices], 
+            torque=[self._rs_core.joint_data.torque[i] for i in head_joint_indices],
+            acceleration=[self._rs_core.joint_data.acceleration[i] for i in head_joint_indices]
+        )
+    
+    def eef_state(self)->Tuple[EndEffectorState, EndEffectorState]:
+        """获取机器人末端执行器的当前状态。
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: 包含左右末端执行器状态的元组。
+                每个EndEffectorState包含:
+                    - position: (float, float, float) ，XYZ位置，单位为米
+                    - orientation: (float, float, float, float) ，四元数方向
+                    - state: EndEffectorState.GraspingState ，当前抓取状态 (UNKNOWN, OPEN, CLOSED)
+        """
+        return copy.deepcopy(self._rs_core.eef_state)
+
+    def gait_name(self)->str:
+        """获取机器人的当前步态名称。
+
+        Returns:
+            str: 当前步态的名称，例如 'trot'、'walk'、'stance'、'custom_gait'。
+        """
+        return self._rs_core.gait_name()
+    
+
+    def is_stance(self) -> bool:
+        """检查机器人当前是否处于站立模式。
+
+        Returns:
+            bool: 如果机器人处于站立模式返回True，否则返回False。
+        """
+        return self._rs_core.is_gait('stance')
+
+    def is_walk(self) -> bool:
+        """检查机器人当前是否处于行走模式。
+
+        Returns:
+            bool: 如果机器人处于行走模式返回True，否则返回False。
+        """
+        return self._rs_core.is_gait('walk')
+
+    def is_step_control(self) -> bool:
+        """检查机器人当前是否处于单步控制模式。
+
+        Returns:
+            bool: 如果机器人处于单步控制模式返回True，否则返回False。
+        """
+        return self._rs_core.is_gait('custom_gait')
+
+    def wait_for_stance(self, timeout:float=5.0)->bool:
+        """等待机器人进入站立模式。
+
+        Args:
+            timeout (float): 等待机器人进入站立状态的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入站立状态返回True，否则返回False。
+        """
+        wait_time = 0
+        while not self._rs_core.is_gait('stance') and wait_time < timeout:
+            time.sleep(0.1)
+            wait_time += 0.1
+        return self._rs_core.is_gait('stance')
+    
+    def wait_for_trot(self, timeout:float=5.0)->bool:
+        """等待机器人进入踏步状态。
+
+        Args:
+            timeout (float): 等待机器人进入踏步状态的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入踏步状态返回True，否则返回False。
+        """
+        return self.wait_for_walk(timeout=timeout)
+
+    def wait_for_walk(self, timeout:float=5.0)->bool:
+        """等待机器人进入行走模式。
+
+        Args:
+            timeout (float): 等待机器人进入行走状态的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入行走状态返回True，否则返回False。
+        """
+        wait_time = 0
+        while not self._rs_core.is_gait('walk') and wait_time < timeout:
+            time.sleep(0.1)
+            wait_time += 0.1
+        return self._rs_core.is_gait('walk')
+
+    def wait_for_step_control(self, timeout:float=5.0)->bool:
+        """等待机器人进入单步控制模式。
+
+        Args:
+            timeout (float): 等待机器人进入单步控制模式的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入单步控制模式返回True，否则返回False。
+        """
+        wait_time = 0
+        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')
+    
+# 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/robot_tool.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix, KuavoPose)
+from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCore
+from typing import Union, Tuple
+
+class KuavoRobotTools:
+    """机器人工具类,提供坐标系转换接口。
+    
+    该类封装了不同机器人坐标系之间的坐标变换查询功能,支持多种返回数据格式。
+    """
+    
+    def __init__(self):
+        self.tools_core = KuavoRobotToolsCore()
+
+    def get_tf_transform(self, target_frame: str, source_frame: str, 
+                       return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """获取指定坐标系之间的变换。
+        
+        Args:
+            target_frame (str): 目标坐标系名称
+            source_frame (str): 源坐标系名称
+            return_type (str, optional): 返回数据格式类型。有效值: \n
+                - "pose_quaternion" : 四元数姿态格式,  \n
+                - "homogeneous" : 齐次矩阵格式。默认为"pose_quaternion"。\n
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 
+                指定格式的变换数据,如果失败则返回None
+        
+        Raises:
+            ValueError: 如果提供了无效的 return_type
+        """
+        return self.tools_core._get_tf_tree_transform(target_frame, source_frame, return_type=return_type)
+
+    def get_base_to_odom(self, return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """获取从base_link到odom坐标系的变换。
+        
+        Args:
+            return_type (str, optional): 返回格式类型。与get_tf_transform相同，默认为"pose_quaternion"。
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 变换数据或 None
+        """
+        return self.get_tf_transform("odom", "base_link", return_type)
+
+    def get_camera_to_base(self, return_type: str = "homogeneous") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """获取从camera_link到base_link坐标系的变换。
+        
+        Args:
+            return_type (str, optional): 返回格式类型。与get_tf_transform相同，默认为"homogeneous"。
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 变换数据或None
+        """
+        return self.get_tf_transform("base_link", "camera_link", return_type)
+
+    def get_link_position(self, link_name: str, reference_frame: str = "base_link") -> Union[Tuple[float, float, float], None]:
+        """获取指定机械臂关节链接的位置
+        
+        Args:
+            link_name (str): 关节链接名称，如"zarm_l1_link"
+            reference_frame (str): 参考坐标系，默认为base_link
+            
+        Returns:
+            Tuple[float, float, float] | None: 三维位置坐标(x,y,z)，失败返回None
+        """
+        try:
+            # 获取从参考坐标系到目标链接的变换
+            transform = self.tools_core._get_tf_tree_transform(
+                reference_frame,
+                link_name,
+                return_type="pose_quaternion"
+            )
+            if transform:
+                return transform.position, transform.orientation
+            return None
+        except Exception as e:
+            SDKLogger.error(f"获取{link_name}位置失败: {str(e)}")
+            return None
+
+    def get_link_pose(self, link_name: str, reference_frame: str = "base_link"):
+        """获取指定机械臂关节链接的位置
+
+        Args:
+            link_name (str): 关节链接名称，如"zarm_l1_link"
+            reference_frame (str): 参考坐标系，默认为base_link
+
+        Returns:
+            Tuple[float, float, float] | None: 三维位置坐标(x,y,z)，失败返回None
+        """
+        try:
+            # 获取从参考坐标系到目标链接的变换
+            transform = self.tools_core._get_tf_tree_transform(
+                reference_frame,
+                link_name,
+                return_type="pose_quaternion"
+            )
+            if transform:
+                return KuavoPose(
+                    position=transform.position,
+                    orientation=transform.orientation
+                )
+            return None
+        except Exception as e:
+            SDKLogger.error(f"获取{link_name}位置失败: {str(e)}")
+            return None

kuavo_humanoid_sdk/kuavo/robot_vision.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData)
+from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore
+
+class KuavoRobotVision:
+    """Kuavo机器人视觉系统接口。
+    
+    提供从不同坐标系获取AprilTag检测数据的接口。
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """初始化视觉系统。
+        
+        Args:
+            robot_type (str, optional): 机器人类型标识符。默认为"kuavo"
+        """
+        if not hasattr(self, '_initialized'):
+            self._vision_core = KuavoRobotVisionCore()
+
+    def get_data_by_id(self, target_id: int, data_source: str = "base") -> dict:
+        """获取指定ID的AprilTag检测数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            data_source (str, optional): 数据源坐标系。可以是"base"、"camera"或"odom"。默认为"base"。
+                
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据
+        """
+        return self._vision_core._get_data_by_id(target_id, data_source)
+    
+    def get_data_by_id_from_camera(self, target_id: int) -> dict:
+        """从相机坐标系获取AprilTag数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据。参见 :meth:`get_data_by_id` 的返回格式说明。
+        """
+        return self._vision_core._get_data_by_id(target_id, "camera")
+    
+    def get_data_by_id_from_base(self, target_id: int) -> dict:
+        """从基座坐标系获取AprilTag数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据。参见 :meth:`get_data_by_id` 的返回格式说明。
+        """
+        return self._vision_core._get_data_by_id(target_id, "base")
+    
+    def get_data_by_id_from_odom(self, target_id: int) -> dict:
+        """从里程计坐标系获取AprilTag数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据。参见 :meth:`get_data_by_id` 的返回格式说明。
+        """
+        return self._vision_core._get_data_by_id(target_id, "odom")
+    
+    @property
+    def apriltag_data_from_camera(self) -> AprilTagData:
+        """AprilTagData: 相机坐标系中检测到的所有AprilTag（属性）"""
+        return self._vision_core.apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self) -> AprilTagData:
+        """AprilTagData: 基座坐标系中检测到的所有AprilTag（属性）"""
+        return self._vision_core.apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self) -> AprilTagData:
+        """AprilTagData: 里程计坐标系中检测到的所有AprilTag（属性）"""
+        return self._vision_core.apriltag_data_from_odom

kuavo_humanoid_sdk/kuavo_strategy/__init__.py

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