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

kuavo_humanoid_sdk/kuavo_strategy/kuavo_strategy.py

@@ -0,0 +1,106 @@
+from abc import ABC, abstractmethod
+from kuavo_humanoid_sdk import KuavoSDK, KuavoRobot
+from kuavo_humanoid_sdk import KuavoRobotState
+from kuavo_humanoid_sdk import KuavoRobotTools
+from kuavo_humanoid_sdk import KuavoRobotVision
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose
+
+class KuavoRobotStrategyBase(ABC):
+    """Kuavo机器人策略基础类，提供策略执行的抽象接口"""
+    
+    def __init__(self, robot:KuavoRobot, robot_state:KuavoRobotState, robot_tools:KuavoRobotTools, robot_vision:KuavoRobotVision):
+        """初始化策略基础类
+        
+        Args:
+            robot: KuavoRobot实例，提供机器人控制能力
+            robot_state: KuavoRobotState实例，提供机器人状态信息
+            robot_tools: KuavoRobotTools实例，提供坐标转换等工具
+            robot_vision: KuavoRobotVision实例，提供视觉感知能力
+        """
+        self.robot = robot
+        self.state = robot_state 
+        self.tools = robot_tools 
+        self.vision = robot_vision 
+    
+    @abstractmethod
+    def head_find_target(self, target_info, **kwargs):
+        """寻找特定ID的目标
+        
+        Args:
+            target_id: 目标的ID标识
+            **kwargs: 其他参数
+            
+        Returns:
+            bool: 是否成功找到目标
+        """
+        pass
+    
+    @abstractmethod
+    def walk_approach_target(self, target_id, target_distance=0.5, **kwargs):
+        """走/接近特定的目标到指定距离
+        
+        Args:
+            target_id: 目标的ID标识
+            target_distance: 与目标的期望距离(米)
+            **kwargs: 其他参数
+            
+        Returns:
+            bool: 是否成功接近目标
+        """
+        pass
+    
+    @abstractmethod
+    def walk_to_pose(self, target_pose:KuavoPose, target_distance=0.5, timeout=10.0, **kwargs):
+        """走到指定距离的目标位置
+        
+        Args:
+            target_pose: 目标位姿
+            target_distance: 与目标的期望距离(米)
+            timeout: 超时时间(秒)
+            **kwargs: 其他参数
+            
+        Returns:
+            bool: 是否成功接近目标
+        """
+        pass
+
+    @abstractmethod
+    def arm_move_to_target(self, target_info, **kwargs):
+        """手臂移动到特定的位置
+        
+        Args:
+            target_info: 目标信息，包含位置、姿态等
+            arm_mode: 手臂控制模式
+            **kwargs: 其他参数
+            
+        Returns:
+            bool: 是否成功移动到目标位置
+        """
+        pass
+
+    @abstractmethod
+    def arm_transport_target_up(self, target_info, **kwargs):
+        """提起操作对象
+        
+        Args:
+            target_info: 目标信息，包含位置、姿态等
+            arm_mode: 手臂控制模式
+            **kwargs: 其他参数
+            
+        Returns:
+            bool: 是否成功提起操作对象
+        """
+        pass
+
+    @abstractmethod
+    def arm_transport_target_down(self, target_info, **kwargs):
+        """放下操作对象
+        
+        Args:
+            target_info: 目标信息，包含位置、姿态等
+            arm_mode: 手臂控制模式
+            **kwargs: 其他参数
+            
+        Returns:
+            bool: 是否成功放下操作对象
+        """

kuavo_humanoid_sdk/kuavo_strategy_v2/common/data_type.py

@@ -0,0 +1,340 @@
+import numpy as np
+from typing import Tuple
+from scipy.spatial.transform import Rotation as R
+from enum import Enum
+
+from kuavo_humanoid_sdk.kuavo_strategy_v2.utils.utils import quaternion_rotate, quaternion_multiply, quaternion_inverse
+
+
+class Frame(str, Enum):
+    BASE = "base_link"  # 基座坐标系
+    CAMERA_LINK = "camera_link"  # 相机坐标系
+    ODOM = "odom"  # 里程计原点
+    # WORLD = "world"  # 世界坐标系
+    ROBOT = "robot"  # 机器人坐标系
+    TAG = "tag"  # AprilTag坐标系
+
+
+class Point:
+    def __init__(self, x: float, y: float, z: float, frame: Frame):
+        """
+        初始化三维坐标点。
+
+        参数：
+            x (float): x坐标。
+            y (float): y坐标。
+            z (float): z坐标。
+            frame (Frame): 坐标系框架名称。
+        """
+        self.x = x
+        self.y = y
+        self.z = z
+        self.frame = frame  # 坐标系框架名称
+
+    def __repr__(self):
+        """
+        返回Point对象的字符串表示。
+        """
+        return f"Point(x={self.x}, \n y={self.y}, \n z={self.z}, \n frame={self.frame})"
+
+
+class Pose:
+    def __init__(self,
+                 pos: Tuple[float, float, float],
+                 quat: Tuple[float, float, float, float],
+                 frame: str = None,
+                 ):
+        """
+        初始化姿态对象，包含位置和四元数。
+
+        参数：
+            pos (Tuple[float, float, float]): 位置坐标。
+            quat (Tuple[float, float, float, float]): 四元数 (x, y, z, w)。
+            frame (str): 坐标系框架名称，可选。
+        """
+        self.pos = np.array(pos)
+        self.quat = np.array(quat)
+        self.frame = frame  # 坐标系框架名称，可选
+
+    @classmethod
+    def from_euler(cls, pos: Tuple[float, float, float], euler: Tuple[float, float, float], frame=None, degrees=True):
+        """
+        从欧拉角创建Pose对象。
+
+        参数：
+            pos (Tuple[float, float, float]): 位置坐标。
+            euler (Tuple[float, float, float]): 欧拉角。
+            frame (str): 坐标系框架名称，可选。
+            degrees (bool): 欧拉角是否为度数。
+
+        返回：
+            Pose: 创建的Pose对象。
+        """
+        r = R.from_euler('xyz', euler, degrees=degrees)
+        quat = r.as_quat()  # x, y, z, w
+        # array to tuple
+        return cls(pos, quat, frame)
+
+    @classmethod
+    def from_rotmat(cls, pos: Tuple[float, float, float], rotmat: Tuple[float, float, float], frame=None):
+        """
+        从旋转矩阵创建Pose对象。
+
+        参数：
+            pos (Tuple[float, float, float]): 位置坐标。
+            rotmat (Tuple[float, float, float]): 旋转矩阵。
+            frame (str): 坐标系框架名称，可选。
+
+        返回：
+            Pose: 创建的Pose对象。
+        """
+        r = R.from_matrix(rotmat)
+        quat = r.as_quat()
+        return cls(pos, quat, frame)
+
+    def get_quat(self):
+        """
+        获取姿态的四元数。
+
+        返回：
+            np.ndarray: 四元数。
+        """
+        return self.quat
+
+    def get_euler(self, degrees=False):
+        """
+        获取姿态的欧拉角。
+
+        参数：
+            degrees (bool): 是否返回度数。
+
+        返回：
+            np.ndarray: 欧拉角。
+        """
+        r = R.from_quat(self.quat)
+        return r.as_euler('xyz', degrees=degrees)
+
+    def get_rotmat(self):
+        """
+        获取姿态的旋转矩阵。
+
+        返回：
+            np.ndarray: 旋转矩阵。
+        """
+        r = R.from_quat(self.quat)
+        return r.as_matrix()
+
+    def position_l1_norm(self, other: "Pose"):
+        """
+        计算位置的L1范数（曼哈顿距离）。
+
+        参数：
+            other (Pose): 另一个姿态对象。
+
+        返回：
+            float: L1范数。
+        """
+        return np.sum(np.abs(self.pos - other.pos))
+
+    def position_l2_norm(self, other: "Pose"):
+        """
+        计算位置的L2范数（欧氏距离）。
+
+        参数：
+            other (Pose): 另一个姿态对象。
+
+        返回：
+            float: L2范数。
+        """
+        return np.linalg.norm(self.pos - other.pos)
+
+    def position_l2_norm_squared(self, other: "Pose"):
+        """
+        计算位置的L2范数的平方。
+
+        参数：
+            other (Pose): 另一个姿态对象。
+
+        返回：
+            float: L2范数的平方。
+        """
+        return np.sum((self.pos - other.pos) ** 2)
+
+    def angle(self, other: "Pose"):
+        """
+        计算姿态的角度差（弧度）。
+
+        参数：
+            other (Pose): 另一个姿态对象。
+
+        返回：
+            float: 角度差。
+        """
+        # 确保四元数已归一化
+        quat1_norm = np.linalg.norm(self.quat)
+        quat2_norm = np.linalg.norm(other.quat)
+
+        if quat1_norm == 0 or quat2_norm == 0:
+            return 0.0
+
+        # 计算归一化的四元数点积
+        quat1_normalized = self.quat / quat1_norm
+        quat2_normalized = other.quat / quat2_norm
+
+        quat_dot = np.abs(np.dot(quat1_normalized, quat2_normalized))
+
+        # 确保点积在有效范围内 [-1, 1]
+        quat_dot = np.clip(quat_dot, -1.0, 1.0)
+
+        return 2 * np.arccos(quat_dot)
+
+    def angle_yaw(self, other: "Pose"):
+        """
+        计算姿态的角度差（弧度）。
+        只考虑yaw的误差。用于2d情况下的计算。
+        参数：
+            other (Pose): 另一个姿态对象。
+
+        返回：
+            float: 角度差。
+        """
+        # 确保四元数已归一化
+        # quat1_norm = np.linalg.norm(self.quat)
+        # quat2_norm = np.linalg.norm(other.quat)
+        #
+        # if quat1_norm == 0 or quat2_norm == 0:
+        #     return 0.0
+        #
+        # # 计算归一化的四元数点积
+        # quat1_normalized = self.quat / quat1_norm
+        # quat2_normalized = other.quat / quat2_norm
+
+        quat1_normalized = R.from_quat(self.quat)
+        quat2_normalized = R.from_quat(other.quat)
+
+        # 相对旋转：从 self 变换到 other
+        r_rel = quat2_normalized * quat1_normalized.inv()
+        print(r_rel)
+        # 转换为欧拉角
+        yaw = r_rel.as_euler("xyz", degrees=False)[2]
+        return yaw
+
+    def __repr__(self):
+        """
+        返回Pose对象的字符串表示。
+        """
+        return f"Pose(pos={self.pos}, \n quat={self.quat}, \n euler={self.get_euler(degrees=True)}, \n frame={self.frame})"
+
+
+class Tag:
+    def __init__(self, id: int, pose: Pose, size: float = None):
+        """
+        初始化AprilTag对象。
+
+        参数：
+            id (int): AprilTag ID。
+            pose (Pose): Tag的Pose对象，包含位置和方向。
+            size (float): AprilTag物理尺寸，单位米。
+        """
+        self.id = id
+        self.size = size
+        self.pose = pose
+
+    def __repr__(self):
+        """
+        返回Tag对象的字符串表示。
+        """
+        return f"Tag(id={self.id}, size={self.size}, pose={self.pose})"
+
+class Transform3D:
+    def __init__(self, trans_pose: Pose, source_frame: Frame, target_frame: Frame):
+        """
+        初始化3D变换对象，通过translation和rotation来定义。
+
+        参数：
+            trans_pose (Pose): 变换的Pose对象。
+            source_frame (Frame): 被转换数据所在的坐标系。
+            target_frame (Frame): 要转到的坐标系。
+        """
+        assert trans_pose.frame == target_frame, f"转换的目标坐标系{target_frame}必须与传入的Pose的frame {trans_pose.frame}一致"
+        self.trans_pose = trans_pose
+        self.source_frame = source_frame  # 源坐标系
+        self.target_frame = target_frame  # 目标坐标系
+
+    def apply_to_pose_inverse(self, pose: Pose):
+        """
+        将Pose从target_frame逆变换到source_frame。
+        """
+
+        assert pose.frame == self.target_frame, "逆变换要求Pose的frame必须是target_frame"
+
+        # 1. 姿态逆变换
+        quat_pose = np.array(pose.quat)
+        quat_trans = np.array(self.trans_pose.quat)
+        quat_trans_inv = quaternion_inverse(quat_trans)
+        quat_new = quaternion_multiply(quat_trans_inv, quat_pose)
+
+        # 2. 位置逆变换
+        # pos_diff = pose.pos - trans.pos，然后用 trans.quat 的逆旋转回来
+        pos_diff = np.array(pose.pos) - np.array(self.trans_pose.pos)
+        pos_new = quaternion_rotate(quat_trans_inv, pos_diff)
+
+        return Pose(
+            pos=pos_new,
+            quat=quat_new,
+            frame=self.source_frame
+        )
+
+    def apply_to_pose(self, pose: Pose):
+        """
+        将Pose从source_frame转换到target_frame。
+
+        参数：
+            pose (Pose): 要转换的Pose对象。
+
+        返回：
+            Pose: 转换后的Pose对象。
+        """
+        assert pose.frame == self.source_frame, "转换的目标坐标系必须与传入的Pose的frame一致"
+        # 位置转换
+        rotated_pos = quaternion_rotate(
+            np.array(self.trans_pose.quat),  # 确保四元数是numpy数组
+            pose.pos
+        )
+        pos = np.asarray(self.trans_pose.pos) + rotated_pos
+
+        # 姿态转换（显式转换为numpy数组）
+        quat_1 = np.array(pose.quat)
+        quat_2 = np.array(self.trans_pose.quat)
+        quat = quaternion_multiply(quat_2, quat_1)
+
+        return Pose(
+            pos=pos.tolist(),
+            quat=quat,
+            frame=self.target_frame  # 返回转换后的坐标系
+        )
+
+    def apply_to_point(self, point: Point):
+        """
+        将Point从source_frame转换到target_frame。
+
+        参数：
+            point (Point): 要转换的Point对象。
+
+        返回：
+            Point: 转换后的Point对象。
+        """
+        assert point.frame == self.source_frame, "转换的目标坐标系必须与传入的Point的frame一致"
+        rotated_pos = quaternion_rotate(
+            np.array(self.trans_pose.quat),  # 确保四元数是numpy数组
+            [point.x, point.y, point.z]  # 将Point转换为列表
+        )
+        pos = np.asarray(self.trans_pose.pos) + rotated_pos
+        pos = pos.tolist()
+
+        return Point(
+            x=pos[0],
+            y=pos[1],
+            z=pos[2],
+            frame=self.target_frame  # 返回转换后的坐标系
+        )

kuavo_humanoid_sdk/kuavo_strategy_v2/common/events/base_event.py

@@ -0,0 +1,215 @@
+import time
+from enum import Enum
+from typing import Any
+import logging
+import colorlog
+
+
+class EventStatus(Enum):
+    IDLE = "idle"  # 空闲状态，事件未开始
+    RUNNING = "running"  # 事件正在进行中
+    SUCCESS = "success"
+
+    FAILED = "failed"
+    CLOSED = "closed"  # 事件已停止
+    TIMEOUT = "timeout"  # 事件超时
+
+
+class BaseEvent:
+    """
+    事件：
+    单一的输入
+    有状态判断
+    """
+
+    def __init__(self,
+                 event_name,
+                 ):
+        """
+        初始化事件，设置事件名称并将初始状态设置为IDLE。
+
+        参数：
+            event_name (str): 事件名称。
+        """
+        self.event_name = event_name
+        self.status = EventStatus.IDLE  # 事件状态，初始为IDLE
+        self.start_time = None  # 事件初始状态
+        self.target = None  # 事件目标，初始为None
+
+        self.logger = logging.getLogger(self.event_name)
+        self.logger.setLevel(logging.INFO)
+        # 避免重复添加 handler
+        if not self.logger.handlers:
+            handler = colorlog.StreamHandler()
+            formatter = colorlog.ColoredFormatter(
+                fmt='%(log_color)s[%(asctime)s] [事件：%(name)s] %(levelname)s: %(message)s',
+                datefmt='%H:%M:%S',
+                log_colors={
+                    'DEBUG':    'cyan',
+                    'INFO':     'green',
+                    'WARNING':  'yellow',
+                    'ERROR':    'red',
+                    'CRITICAL': 'bold_red',
+                }
+            )
+            handler.setFormatter(formatter)
+            self.logger.addHandler(handler)
+
+    def set_timeout(self, timeout):
+        """
+        设置事件的超时时间。
+
+        参数：
+            timeout (int): 超时时间（秒），必须大于0。
+        """
+        assert timeout > 0, "超时时间必须大于0 !!!"
+        self.timeout = timeout
+        self.logger.info(f"事件 {self.event_name} 超时时间设置为 {self.timeout} 秒")
+
+    def set_target(self, target: Any, *args, **kwargs):
+        """
+        设置事件的目标，可以在事件执行期间动态更新。
+
+        参数：
+            target (Any): 事件目标，例如位置或ID。
+            `*args`: 额外的参数。
+            `**kwargs`: 额外的关键字参数。
+
+        返回：
+            bool: 如果目标设置成功返回True，否则返回False。
+        """
+
+        if self.status == EventStatus.CLOSED:
+            self.logger.error(f"事件 {self.event_name} 已关闭，无法设置目标 !!! 请先调用open() 方法开始事件")
+            return False
+
+        if self.status != EventStatus.RUNNING and self.status != EventStatus.IDLE:
+            self.logger.error(f"事件 {self.event_name} 不是运行中或空闲状态，无法设置目标 !!!")
+            return False
+
+        is_valid = self._check_target_valid(target, *args, **kwargs)
+
+        if not is_valid:
+            self.logger.error(f"事件 {self.event_name} 的目标无效，无法设置目标 !!!")
+            return False
+
+        self.target = target
+
+        self.logger.info(f"目标已设置为：\n {self.target}")
+
+        return True
+
+    def open(self, *args, **kwargs):
+        """
+        开始事件，将状态更改为RUNNING并记录开始时间。
+
+        参数：
+            `*args`: 额外的参数。
+            `**kwargs`: 额外的关键字参数。
+        """
+        self.status = EventStatus.RUNNING
+        self.start_time = time.time()  # 记录事件开始时间
+        self.logger.info(f"🔵 事件开始啦")
+
+    def close(self):
+        """
+        停止事件，将状态更改为CLOSED。
+        """
+        self.status = EventStatus.CLOSED
+        self.logger.info(f"🔵 事件关闭啦")
+
+    def step(self):
+        """
+        抽象方法，需要在子类中实现以定义事件的每一步行为。
+
+        异常：
+            NotImplementedError: 如果在子类中未实现。
+        """
+        raise NotImplementedError("请在子类中实现 step 方法")
+
+    def _check_target_valid(self, target: Any, *args, **kwargs) -> bool:
+        """
+        抽象方法，需要在子类中实现以验证事件的目标。
+
+        参数：
+            target (Any): 需要验证的目标。
+            `*args`: 额外的参数。
+            `**kwargs`: 额外的关键字参数。
+
+        返回：
+            bool: 如果目标有效返回True，否则返回False。
+
+        异常：
+            NotImplementedError: 如果在子类中未实现。
+        """
+        raise NotImplementedError("请在子类中实现 _check_target_valid 方法")
+
+    def get_status(self):
+        """
+        返回更新后的事件状态。
+
+        返回：
+            EventStatus: 当前事件状态。
+        """
+        self._update_status()
+        return self.status
+
+    def _update_status(self):
+        """
+        根据当前状态更新事件的状态，检查是否失败、成功或超时。
+        """
+        # 如果已经是failed或者success或者timeout，则不再变化状态
+        if self.status in [EventStatus.FAILED, EventStatus.SUCCESS, EventStatus.TIMEOUT]:
+            return
+
+        if self._check_failed():
+            self.logger.error(f"❌ 当前状态为：失败 !!!")
+            self.status = EventStatus.FAILED
+        elif self._check_success():
+            self.logger.info(f"✅ 当前状态为：成功 !!!")
+            self.status = EventStatus.SUCCESS
+        elif self._check_timeout():
+            self.logger.error(f"❌ 当前状态为：超时 !!!")
+            self.status = EventStatus.TIMEOUT
+
+    def _check_timeout(self):
+        """
+        检查事件是否超时。
+
+        返回：
+            bool: 如果事件超时返回True，否则返回False。
+
+        异常：
+            AssertionError: 如果事件未开始或未设置超时时间。
+        """
+        assert self.start_time is not None, "事件未开始，无法检查超时 !!!"
+        assert self.timeout is not None, "事件超时时间未设置 !!!"
+
+        elapsed_time = time.time() - self.start_time
+        if elapsed_time > self.timeout:
+            return True
+        return False
+
+    def _check_failed(self):
+        """
+        抽象方法，需要在子类中实现以判断事件是否失败。
+
+        返回：
+            bool: 如果事件失败返回True，否则返回False。
+
+        异常：
+            NotImplementedError: 如果在子类中未实现。
+        """
+        raise NotImplementedError("请在子类中实现 _check_failed 方法")
+
+    def _check_success(self):
+        """
+        抽象方法，需要在子类中实现以判断事件是否成功。
+
+        返回：
+            bool: 如果事件成功返回True，否则返回False。
+
+        异常：
+            NotImplementedError: 如果在子类中未实现。
+        """
+        raise NotImplementedError("请在子类中实现 _check_success 方法")

kuavo_humanoid_sdk/kuavo_strategy_v2/common/robot_sdk.py

@@ -0,0 +1,25 @@
+import time
+
+from kuavo_humanoid_sdk import KuavoSDK, KuavoRobot
+from kuavo_humanoid_sdk import KuavoRobotState
+from kuavo_humanoid_sdk import KuavoRobotTools
+from kuavo_humanoid_sdk import KuavoRobotVision
+from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm
+
+class RobotSDK:
+    def __init__(self):
+        """
+        初始化机器人SDK
+
+        Args:
+            robot: KuavoRobot实例，提供机器人控制能力
+            robot_state: KuavoRobotState实例，提供机器人状态信息
+            robot_tools: KuavoRobotTools实例，提供坐标转换等工具
+        """
+        self.control = KuavoRobot()
+        self.state = KuavoRobotState()
+        self.tools = KuavoRobotTools()
+        self.vision = KuavoRobotVision()
+        self.arm = KuavoRobotArm()
+
+        time.sleep(2)  # 等待底层初始化完成