kuavo-humanoid-sdk-ws 1.2.0__20250714105233-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/common/websocket_kuavo_sdk.py

@@ -0,0 +1,26 @@
+import roslibpy
+
+class WebSocketKuavoSDK:
+
+    _instance = None
+    _initialized = False
+
+    websocket_host = '127.0.0.1'
+    websocket_port = 9090
+    websocket_timeout = 5.0
+
+    def __new__(cls, *args, **kwargs):
+        if cls._instance is None:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+
+    def __init__(self):
+        if not self._initialized:
+            self._initialized = True
+            self.client = roslibpy.Ros(host=WebSocketKuavoSDK.websocket_host, port=WebSocketKuavoSDK.websocket_port)
+            self.client.run(timeout=WebSocketKuavoSDK.websocket_timeout)
+            
+
+    def __del__(self):
+        self.client.terminate()
+        self.instance = None

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,293 @@
+from typing import Tuple
+from enum import Enum
+from dataclasses import dataclass
+import numpy as np
+
+@dataclass
+class KuavoJointData:
+    """Data class representing joint states of the robot.
+
+    Args:
+        position (list): List of joint positions (angles) in radians
+        velocity (list): List of joint velocities in radians/second
+        torque (list): List of joint torques/efforts in Newton-meters or Amperes
+        acceleration (list): List of joint accelerations in radians/second^2
+    """
+    position: list
+    velocity: list
+    torque: list
+    acceleration:list
+
+@dataclass
+class KuavoImuData:
+    """Data class representing IMU (Inertial Measurement Unit) data from the robot.
+
+    Args:
+        gyro (Tuple[float, float, float]): Angular velocity around x, y, z axes in rad/s
+        acc (Tuple[float, float, float]): Linear acceleration in x, y, z axes in m/s^2
+        free_acc (Tuple[float, float, float]): Free acceleration (gravity compensated) in x, y, z axes in m/s^2
+        quat (Tuple[float, float, float, float]): Orientation quaternion (x, y, z, w)
+    """
+    gyro : Tuple[float, float, float]
+    acc : Tuple[float, float, float]
+    free_acc: Tuple[float, float, float]
+    quat: Tuple[float, float, float, float]
+
+@dataclass
+class KuavoOdometry:
+    """Data class representing odometry data from the robot.
+
+    Args:
+        position (Tuple[float, float, float]): Robot position (x, y, z) in world coordinates in meters
+        orientation (Tuple[float, float, float, float]): Robot orientation as quaternion (x, y, z, w)
+        linear (Tuple[float, float, float]): Linear velocity (x, y, z) in world coordinates in m/s
+        angular (Tuple[float, float, float]): Angular velocity (x, y, z) in world coordinates in rad/s
+    """
+    position: Tuple[float, float, float]
+    orientation: Tuple[float, float, float, float]
+    linear: Tuple[float, float, float]
+    angular: Tuple[float, float, float]
+
+class KuavoArmCtrlMode(Enum):
+    """Enum class representing the control modes for the Kuavo robot arm.
+
+    Attributes:
+        ArmFixed: The robot arm is fixed in position (value: 0)
+        AutoSwing: The robot arm is in automatic swinging mode (value: 1)
+        ExternalControl: The robot arm is controlled by external commands (value: 2)
+    """
+    ArmFixed = 0
+    AutoSwing = 1 
+    ExternalControl = 2
+
+class KuavoManipulationMpcFrame(Enum):
+    """Enum class representing the manipulation mpc frame for the Kuavo robot end effector.
+
+    Attributes:
+        KeepCurrentFrame: Keep the current frame (value: 0)
+        WorldFrame: World frame (value: 1)
+        LocalFrame: Local frame (value: 2)
+        VRFrame: VR frame (value: 3)
+        ManipulationWorldFrame: Manipulation world frame (value: 4)
+    """
+    KeepCurrentFrame = 0
+    WorldFrame = 1
+    LocalFrame = 2
+    VRFrame = 3
+    ManipulationWorldFrame = 4
+    ERROR = -1
+
+class KuavoManipulationMpcCtrlMode(Enum):
+    """Enum class representing the control mode for the Kuavo robot manipulation MPC.
+
+    Attributes:
+        NoControl: No control (value: 0)
+        ArmOnly: Only control the arm (value: 1)
+        BaseOnly: Only control the base (value: 2)
+        BaseArm: Control both the base and the arm (value: 3)
+        ERROR: Error state (value: -1)
+    """
+    NoControl = 0
+    ArmOnly = 1
+    BaseOnly = 2
+    BaseArm = 3
+    ERROR = -1
+
+class KuavoManipulationMpcControlFlow(Enum):
+    """Enum class representing the control data flow for the Kuavo robot manipulation.
+
+    Attributes:
+        ThroughFullBodyMpc: Control data flows through full-body MPC before entering WBC (value: 0)
+        DirectToWbc: Control data flows directly to WBC without full-body MPC (value: 1)
+        Error: Invalid control path (value: -1)
+    """
+    ThroughFullBodyMpc = 0
+    DirectToWbc = 1
+    Error = -1
+
+@dataclass
+class EndEffectorState:
+    """Data class representing the state of the end effector.
+
+    Args:
+        position (list): float, Position of the end effector, range: [0, 100]
+        velocity (list): float, ...
+        effort (list): float, ...
+    """
+    position: list
+    velocity: list
+    effort: list
+    class GraspingState(Enum):
+        """Enum class representing the grasping states of the end effector.
+
+        Attributes:
+            ERROR: Error state (value: -1)
+            UNKNOWN: Unknown state (value: 0)
+            REACHED: Target position reached (value: 1)
+            MOVING: Moving to target position (value: 2)
+            GRABBED: Object successfully grasped (value: 3)
+        """
+        ERROR = -1
+        UNKNOWN = 0
+        MOVING = 1
+        REACHED = 2
+        GRABBED = 3
+
+    state: GraspingState # gripper grasping states
+
+class EndEffectorSide(Enum):
+    """Enum class representing the sides of the end effector.
+
+    Attributes:
+        LEFT: The left side of the end effector (value: 'left')
+        RIGHT: The right side of the end effector (value: 'right')
+        BOTH: Both sides of the end effector (value: 'both')
+    """
+    LEFT = 'left'
+    RIGHT = 'right'
+    BOTH = 'both'
+
+@dataclass
+class KuavoPose:
+    """Data class representing the pose of the robot."""
+    position: Tuple[float, float, float] # x, y, z
+    orientation: Tuple[float, float, float, float] # x, y, z, w
+
+@dataclass
+class KuavoIKParams:
+    """Data class representing the parameters for the IK node."""
+    # snopt params
+    major_optimality_tol: float = 1e-3
+    major_feasibility_tol: float = 1e-3  
+    minor_feasibility_tol: float = 1e-3
+    major_iterations_limit: float = 100
+    # constraint and cost params
+    oritation_constraint_tol: float = 1e-3
+    pos_constraint_tol: float = 1e-3 # 0.001m, work when pos_cost_weight==0.0
+    pos_cost_weight: float = 0.0 # If U need high accuracy, set this to 0.0 !!!
+
+@dataclass
+class KuavoDexHandTouchState:
+    """Data class representing the touch state of the dexterous hand."""
+    
+    @dataclass
+    class KuavoTouchState:
+        """Data class representing the touch state of the dexterous hand."""
+        normal_force1: int  # 法向力1
+        normal_force2: int  # 法向力2
+        normal_force3: int  # 法向力3
+        tangential_force1: int  # 切向力1
+        tangential_force2: int  # 切向力2
+        tangential_force3: int  # 切向力3
+        tangential_direction1: int  # 切向力方向1
+        tangential_direction2: int  # 切向力方向2
+        tangential_direction3: int  # 切向力方向3
+        self_proximity1: int  # 自电容接近传感器1
+        self_proximity2: int  # 自电容接近传感器2
+        mutual_proximity: int  # 互电容接近传感器
+        status: int  # 传感器状态
+    # 5 fingers
+    data: Tuple[KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState, KuavoTouchState]
+
+@dataclass
+class AprilTagData:
+    """Represents detected AprilTag information with pose estimation.
+    
+    Attributes:
+        id (list): List of detected AprilTag IDs (integers)
+        size (list): List of tag physical sizes in meters (floats)
+        pose (list): List of PoseQuaternion objects representing tag poses
+    """
+    id: list
+    size: list
+    pose: list
+
+@dataclass
+class HomogeneousMatrix:
+    """4x4 homogeneous transformation matrix for 3D transformations.
+    
+    Represents both rotation and translation in 3D space. Can be used for
+    coordinate frame transformations and pose composition.
+    
+    Attributes:
+        matrix (np.ndarray): 4x4 numpy array of shape (4, 4) containing::
+        
+            [[R, t],
+             [0, 1]] 
+            
+        where R is 3x3 rotation matrix and t is 3x1 translation
+    """
+    matrix: np.ndarray  # Shape (4,4) homogeneous transformation matrix
+
+@dataclass
+class PoseQuaternion:
+    """3D pose representation using position and quaternion orientation.
+    
+    Provides a singularity-free orientation representation. Commonly used
+    in robotics for smooth interpolation between orientations.
+    
+    Attributes:
+        position (Tuple[float, float, float]): XYZ coordinates in meters
+        orientation (Tuple[float, float, float, float]): Unit quaternion in
+            (x, y, z, w) format following ROS convention
+    """
+    position: Tuple[float, float, float]
+    orientation: Tuple[float, float, float, float]
+
+
+@dataclass
+class KuavoJointCommand:
+    """Data class representing joint command for the robot.
+
+    Args:
+        joint_q (list): List of joint position commands in radians
+        joint_v (list): List of joint velocity commands in radians/second
+        tau (list): List of joint torque/effort commands in Newton-meters or Amperes
+        tau_max (list): List of maximum allowable torques for each joint
+        tau_ratio (list): List of torque ratios (actual/maximum) for each joint
+        joint_kp (list): List of position control proportional gains
+        joint_kd (list): List of velocity control derivative gains
+        control_modes (list): List of control mode integers for each joint
+    """
+    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:
+    """Data class representing twist (velocity) data for the robot.
+
+    Args:
+        linear (Tuple[float, float, float]): Linear velocity (x, y, z) 
+        angular (Tuple[float, float, float]): Angular velocity (x, y, z)
+    """
+    linear: Tuple[float, float, float]
+    angular: Tuple[float, float, float]
+
+
+
+@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,11 @@
+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

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 AudioWebsocket
+
+class KuavoRobotAudioCore:
+    def __init__(self):
+        self.robot_audio = AudioWebsocket()
+
+    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)
+