kuavo-humanoid-sdk 1.2.2b3208__20250922170818-py3-none-any.whl

kuavo_humanoid_sdk/__init__.py

@@ -0,0 +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/common/logger.py

@@ -0,0 +1,45 @@
+import logging
+import os
+from pathlib import Path
+from logging.handlers import RotatingFileHandler
+def setup_logger():
+    logger = logging.getLogger('kuavo-humanoid-sdk')
+    logger.setLevel(logging.DEBUG)
+    log_suffix = f'log/kuavo_humanoid_sdk'
+    log_dir = f'/var/{log_suffix}'
+    try:
+        # Check if we have write permission for /var directory
+        if not os.access('/var/log/', os.W_OK):
+            log_dir = f'./{log_suffix}'
+        Path(log_dir).mkdir(parents=True, exist_ok=True)
+    except Exception as e:
+        # If creation in /var fails, create in current directory
+        log_dir = f'./{log_suffix}'
+        Path(log_dir).mkdir(parents=True, exist_ok=True)        
+    log_file = f'{log_dir}/kuavo_humanoid_sdk.log'
+
+    print(f'kuavo-humanoid-sdk log_file: {log_file}')
+
+    fh = RotatingFileHandler(log_file, maxBytes=2*1024*1024, backupCount=5)  # 每个日志文件最大 2 MB，保留 5 个备份文件
+    fh.setLevel(logging.DEBUG)
+    
+    ch = logging.StreamHandler()
+    ch.setLevel(logging.DEBUG)
+    
+    formatter = logging.Formatter('%(asctime)s - %(levelname)s - %(message)s')
+    fh.setFormatter(formatter)
+    ch.setFormatter(formatter)
+    
+    logger.addHandler(fh)
+    logger.addHandler(ch)
+    return logger
+
+def disable_sdk_logging():
+    """
+        Disable SDK logging.
+    """
+    logging.disable()
+
+
+""" Logger """
+SDKLogger = setup_logger()

kuavo_humanoid_sdk/interfaces/__init__.py

@@ -0,0 +1,4 @@
+from .data_types import *
+from .robot import *
+from .end_effector import *
+from .robot_info import *

kuavo_humanoid_sdk/interfaces/data_types.py

@@ -0,0 +1,288 @@
+from typing import Tuple
+from enum import Enum
+from dataclasses import dataclass
+import numpy as np
+
+@dataclass
+class KuavoJointData:
+    """表示机器人关节状态的数据类"""
+    position: list
+    """关节位置（角度）列表，单位为弧度"""
+    velocity: list
+    """关节速度列表，单位为弧度/秒"""
+    torque: list
+    """关节扭矩/力矩列表，单位为牛顿·米或安培"""
+    acceleration:list
+    """关节加速度列表，单位为弧度/秒²"""
+
+@dataclass
+class KuavoImuData:
+    """表示机器人IMU（惯性测量单元）数据的数据类"""
+    gyro : Tuple[float, float, float]
+    """绕x、y、z轴的角速度，单位为弧度/秒"""
+    acc : Tuple[float, float, float]
+    """x、y、z轴的线性加速度，单位为米/秒²"""
+    free_acc: Tuple[float, float, float]
+    """自由加速度（重力补偿），x、y、z轴，单位为米/秒²"""
+    quat: Tuple[float, float, float, float]
+    """方向四元数 (x, y, z, w)"""
+
+@dataclass
+class KuavoOdometry:
+    """表示机器人里程计数据的数据类"""
+    position: Tuple[float, float, float]
+    """机器人在世界坐标系中的位置 (x, y, z)，单位为米"""
+    orientation: Tuple[float, float, float, float]
+    """机器人方向四元数 (x, y, z, w)"""
+    linear: Tuple[float, float, float]
+    """世界坐标系中的线性速度 (x, y, z)，单位为米/秒"""
+    angular: Tuple[float, float, float]
+    """世界坐标系中的角速度 (x, y, z)，单位为弧度/秒"""
+
+class KuavoArmCtrlMode(Enum):
+    """表示Kuavo机器人手臂控制模式的枚举类。"""
+    ArmFixed = 0
+    """手臂固定(冻结)模式"""
+    AutoSwing = 1 
+    """自动摆臂模式"""
+    ExternalControl = 2
+    """手臂由外部命令控制"""
+
+class KuavoManipulationMpcFrame(Enum):
+    """表示Kuavo机器人末端执行器 Manipulation MPC 坐标系的枚举类"""
+    KeepCurrentFrame = 0
+    """保持当前坐标系"""
+    WorldFrame = 1
+    """世界坐标系"""
+    LocalFrame = 2
+    """本地坐标系"""
+    VRFrame = 3
+    """VR坐标系"""
+    ManipulationWorldFrame = 4
+    """操作世界坐标系"""
+    ERROR = -1
+    """错误状态"""
+
+class KuavoManipulationMpcCtrlMode(Enum):
+    """表示Kuavo机器人 Manipulation MPC 控制模式的枚举类"""
+    NoControl = 0
+    """无控制"""
+    ArmOnly = 1
+    """仅控制手臂"""
+    BaseOnly = 2
+    """仅控制底座"""
+    BaseArm = 3
+    """同时控制底座和手臂"""
+    ERROR = -1
+    """错误状态"""
+
+class KuavoManipulationMpcControlFlow(Enum):
+    """表示Kuavo机器人 Manipulation MPC 控制数据流的枚举类"""
+    ThroughFullBodyMpc = 0  
+    """控制数据通过全身MPC后进入WBC"""
+    DirectToWbc = 1
+    """控制数据直接流向WBC，不经过全身MPC"""
+    Error = -1
+    """无效的控制路径"""
+
+@dataclass
+class EndEffectorState:
+    """表示末端执行器状态的数据类。
+
+    参数:
+        position (list): 浮点数，末端执行器位置，范围: [0, 100] \n
+        velocity (list): 浮点数，速度 \n
+        effort (list): 浮点数，力矩 \n
+        state (GraspingState): 夹爪抓取状态 \n
+    """
+    position: list
+    velocity: list
+    effort: list
+    class GraspingState(Enum):
+        """表示末端执行器抓取状态的枚举类。"""
+
+        ERROR = -1
+        """错误状态"""
+        UNKNOWN = 0
+        """未知状态"""
+        MOVING = 1
+        """正在移动到目标位置"""
+        REACHED = 2
+        """到达目标位置"""
+        GRABBED = 3
+        """成功抓取物体"""
+
+    state: GraspingState
+
+class EndEffectorSide(Enum):
+    """表示末端执行器类型的枚举类。"""
+    LEFT = 'left'
+    """左末端执行器"""
+    RIGHT = 'right'
+    """右末端执行器"""
+    BOTH = 'both'
+    """左右末端执行器"""
+
+@dataclass
+class KuavoPose:
+    """表示机器人姿态的数据类。"""
+    position: Tuple[float, float, float] # x, y, z
+    orientation: Tuple[float, float, float, float] # x, y, z, w
+
+    def __str__(self) -> str:
+        """Returns a formatted string representation of the pose.
+        
+        Returns:
+            str: Formatted pose string with position and orientation
+        """
+        return (
+            f"Position (x,y,z): ({self.position[0]:.3f}, {self.position[1]:.3f}, {self.position[2]:.3f})\n"
+            f"Orientation (x,y,z,w): ({self.orientation[0]:.3f}, {self.orientation[1]:.3f}, "
+            f"{self.orientation[2]:.3f}, {self.orientation[3]:.3f})"
+        )
+
+@dataclass
+class KuavoIKParams:
+    """表示IK节点参数的数据类。"""
+    # snopt参数
+    major_optimality_tol: float = 1e-3
+    major_feasibility_tol: float = 1e-3  
+    minor_feasibility_tol: float = 1e-3
+    major_iterations_limit: float = 100
+    # 约束和成本参数
+    oritation_constraint_tol: float = 1e-3
+    pos_constraint_tol: float = 1e-3 # 0.001m，当pos_cost_weight==0.0时生效
+    pos_cost_weight: float = 0.0 # 如果需要高精度，请将此值设为0.0！！！
+
+@dataclass
+class KuavoDexHandTouchState:
+    """表示灵巧手触觉状态的数据类。"""
+    
+    @dataclass
+    class KuavoTouchState:
+        """表示灵巧手触觉状态的数据类"""
+        normal_force1: int  # 法向力1
+        """法向力1"""
+        normal_force2: int  # 法向力2
+        """法向力2"""
+        normal_force3: int  # 法向力3
+        """法向力3"""
+        tangential_force1: int  # 切向力1
+        """切向力1"""
+        tangential_force2: int  # 切向力2
+        """切向力2"""
+        tangential_force3: int  # 切向力3
+        """切向力3"""
+        tangential_direction1: int  # 切向力方向1
+        """切向力方向1"""
+        tangential_direction2: int  # 切向力方向2
+        """切向力方向2"""
+        tangential_direction3: int  # 切向力方向3
+        """切向力方向3"""
+        self_proximity1: int  # 自电容接近传感器1
+        """自电容接近传感器1"""
+        self_proximity2: int  # 自电容接近传感器2
+        """自电容接近传感器2"""
+        mutual_proximity: int  # 互电容接近传感器
+        """互电容接近传感器"""
+        status: int  # 传感器状态
+        """传感器状态"""
+    # 5个手指
+    data: Tuple[KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState]
+
+@dataclass
+class AprilTagData:
+    """表示检测到的AprilTag信息及姿态估计"""
+    id: list
+    """检测到的AprilTag ID列表（整数）"""   
+    size: list
+    """标签物理尺寸列表，单位为米（浮点数）"""
+    pose: list
+    """表示标签姿态的PoseQuaternion对象列表"""
+
+@dataclass
+class HomogeneousMatrix:
+    """用于3D变换的4x4齐次变换矩阵。
+    
+    表示3D空间中的旋转和平移。可用于坐标系变换和姿态合成。 \n
+    
+    属性:
+        matrix (np.ndarray): 形状为(4, 4)的4x4 numpy数组，包含::
+        
+            [[R, t],
+             [0, 1]] 
+            
+        其中R是3x3旋转矩阵，t是3x1平移向量
+    """
+    matrix: np.ndarray  # 形状(4,4)的齐次变换矩阵
+
+@dataclass
+class PoseQuaternion:
+    """使用位置和四元数方向的3D姿态表示"""
+    position: Tuple[float, float, float]
+    """XYZ坐标，单位为米"""
+    orientation: Tuple[float, float, float, float]
+    """单位四元数，采用(x, y, z, w)格式，遵循ROS约定"""
+
+
+@dataclass
+class KuavoJointCommand:
+    """表示机器人关节命令的数据类\n
+    """
+    joint_q: list
+    """关节位置命令列表，单位为弧度"""
+    joint_v: list
+    """关节速度命令列表，单位为弧度/秒"""
+    """关节扭矩/力矩命令列表，单位为牛顿·米或安培"""
+    tau: list
+    """关节扭矩/力矩命令列表，单位为牛顿·米或安培"""
+    tau_max: list
+    """每个关节的最大允许扭矩列表"""
+    tau_ratio: list
+    """每个关节的扭矩比率（实际/最大）列表"""
+    joint_kp: list
+    """位置控制比例增益列表"""
+    joint_kd: list
+    """速度控制微分增益列表"""
+    control_modes: list
+    """每个关节的控制模式整数列表"""
+
+
+@dataclass
+class KuavoTwist:
+    """表示机器人扭转（速度）数据的数据类"""
+    linear: Tuple[float, float, float]
+    """线性速度 (x, y, z)，单位为米/秒"""
+    angular: Tuple[float, float, float]
+    """角速度 (x, y, z)，单位为弧度/秒"""
+
+
+@dataclass
+class KuavoMotorParam:
+    """表示机器人电机参数的数据类"""
+    Kp: float
+    """位置控制比例增益"""
+    Kd: float
+    """速度控制微分增益"""
+    id: int
+    """电机ID"""
+
+
+@dataclass
+class AprilTagDetection:
+    """表示AprilTag检测结果的数据类"""
+    @dataclass
+    class Point:
+        x: float
+        y: float
+        z: float
+
+    @dataclass
+    class Quaternion:
+        x: float
+        y: float
+        z: float
+        w: float
+
+    position: Point
+    orientation: Quaternion

kuavo_humanoid_sdk/interfaces/end_effector.py

@@ -0,0 +1,62 @@
+from abc import ABC, abstractmethod
+from typing import Tuple
+from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState
+    
+class EndEffector(ABC):
+    def __init__(self, joint_names: list):
+        self._joint_names = joint_names
+
+    def joint_names(self)->list:
+        """Returns the joint names of the end effector.
+        
+        Returns:
+            list: The joint names of the end effector.
+        """
+        return self._joint_names
+    
+    def joint_count(self) -> int:
+        """Returns the total number of joints in the end effector.
+        
+        The joint_names list contains joints for both left and right end effectors.
+        For a single end effector, the number of joints would be joint_names.size/2.
+        
+        Returns:
+            int: The total number of joints in the end effector.
+        """
+        return len(self._joint_names)
+    
+    @abstractmethod
+    def control(self, target_positions:list, target_velocities:list, target_torques:list)->bool:
+        pass
+
+    @abstractmethod
+    def control_right(self, target_positions:list, target_velocities:list, target_torques:list)->bool:
+        pass
+
+    @abstractmethod
+    def control_left(self, target_positions:list, target_velocities:list, target_torques:list)->bool:
+        pass    
+
+    @abstractmethod
+    def open(self, side:EndEffectorSide)->bool:
+        pass
+
+    @abstractmethod
+    def get_state(self)->Tuple[EndEffectorState, EndEffectorState]:
+        pass
+
+    @abstractmethod
+    def get_position(self)->Tuple[list, list]:
+        pass
+
+    @abstractmethod
+    def get_velocity(self)->Tuple[list, list]:
+        pass
+
+    @abstractmethod
+    def get_effort(self)->Tuple[list, list]:
+        pass
+
+    @abstractmethod
+    def get_grasping_state(self)->Tuple[EndEffectorState.GraspingState, EndEffectorState.GraspingState]:
+        pass

kuavo_humanoid_sdk/interfaces/robot.py

@@ -0,0 +1,22 @@
+
+from abc import ABC, abstractmethod
+
+class RobotBase(ABC):
+    def __init__(self, robot_type: str = "kuavo"):
+        self._robot_type = robot_type
+
+    @property
+    def robot_type(self) -> str:
+        return self._robot_type
+
+    @abstractmethod
+    def trot(self):
+        pass
+
+    @abstractmethod
+    def stance(self):
+        pass
+
+    @abstractmethod
+    def jump(self):
+        pass

kuavo_humanoid_sdk/interfaces/robot_info.py

@@ -0,0 +1,56 @@
+from abc import ABC, abstractmethod
+
+class RobotInfoBase(ABC):
+    def __init__(self, robot_type: str = "kuavo"):
+        self._robot_type = robot_type
+
+    @property
+    def robot_type(self) -> str:
+        """
+            Get the robot type, e.g. "kuavo"
+        """
+        return self._robot_type
+    
+    @property
+    @abstractmethod
+    def robot_version(self) -> str:
+        """
+            Get the robot version, e.g. "42", "43"
+        """
+        pass
+
+    @property
+    @abstractmethod
+    def end_effector_type(self) -> str:
+        """
+            Get the end effector type, e.g. "lejuclaw"...
+        """
+        pass
+
+    @property
+    @abstractmethod
+    def joint_names(self) -> list:
+        """
+            Get the joint names, e.g. ["joint1", "joint2", ...]
+        """
+        pass
+
+    @property
+    @abstractmethod
+    def joint_dof(self) -> int:
+        """
+            Get the joint degrees of freedom, e.g. 28
+        """
+        pass
+
+    @property
+    @abstractmethod
+    def arm_joint_dof(self) -> int:
+        """
+            Get the arm joint degrees of freedom, e.g. 14
+        """
+        pass
+
+    def __str__(self) -> str:
+       pass
+    

kuavo_humanoid_sdk/kuavo/__init__.py

@@ -0,0 +1,16 @@
+from .robot import KuavoSDK, KuavoRobot  
+from .robot_info import KuavoRobotInfo
+from .robot_state import KuavoRobotState
+from .robot_vision import KuavoRobotVision
+from .robot_tool import KuavoRobotTools
+from .robot_arm import KuavoRobotArm
+from .robot_head import KuavoRobotHead
+from .robot_audio import KuavoRobotAudio
+from .dexterous_hand import DexterousHand, TouchDexterousHand
+from .leju_claw import LejuClaw
+from .robot_observation import KuavoRobotObservation
+from .robot_blockly import RobotControlBlockly
+from .robot_microphone import RobotMicrophone
+from .robot_navigation import RobotNavigation
+from .robot_speech import RobotSpeech
+from .dexterous_hand import DexterousHand

kuavo_humanoid_sdk/kuavo/core/audio.py

@@ -0,0 +1,32 @@
+import time
+import math
+import threading
+import numpy as np
+from typing import Tuple
+from transitions import Machine, State
+
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.ros.audio import Audio
+
+class KuavoRobotAudioCore:
+    def __init__(self):
+        self.robot_audio = Audio()
+
+    def play_audio(self, music_number: str, volume: int = 100, speed: float = 1.0) -> bool:
+        """
+        play music
+        """
+        return self.robot_audio.play_audio(music_number, volume, speed)
+
+    def stop_music(self) -> bool:
+        """
+        stop music
+        """
+        return self.robot_audio.stop_audio()
+
+    def text_to_speech(self, text: str, volume: float = 0.5) -> bool:
+        """
+        text to speech
+        """
+        return self.robot_audio.text_to_speech(text, volume)
+