kuavo-humanoid-sdk-ws 1.3.2b399__20260203103858-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/launch_robot_tool.py

@@ -0,0 +1,170 @@
+import roslibpy
+import time
+from typing import Tuple
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+
+
+class LaunchRobotStatus:
+    # 与硬件节点保持一致
+    UNLAUNCH = 'unlaunch'
+    INITING = 'initing'
+    UNKNOWN = 'unknown'
+    READY_STANCE = 'ready_stance'
+    CALIBRATE = 'calibrate'
+    LAUNCHED = 'launched'
+
+class LaunchRobotTool:
+    def __init__(self):
+        self._websocket = WebSocketKuavoSDK()
+        self._stand_timeout = 10.0
+        self._srv_robot_start = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/start_robot', 'std_srvs/Trigger')
+        self._srv_robot_stand = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/stand_robot', 'std_srvs/Trigger')
+        self._srv_get_robot_launch_status = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/get_robot_launch_status', 'std_srvs/Trigger')
+        self._srv_robot_stop = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/stop_robot', 'std_srvs/Trigger')
+        
+    def is_unlaunch(self, status:str)->bool:
+        return status == LaunchRobotStatus.UNLAUNCH or status == LaunchRobotStatus.UNKNOWN
+    
+    def is_launched(self, status:str)->bool:
+        return status == LaunchRobotStatus.LAUNCHED
+
+    def is_launching(self, status:str)->bool:
+        return self.is_calibrate(status) or self.is_ready_stance(status)
+
+    def is_calibrate(self, status:str)->bool:
+        return status == LaunchRobotStatus.CALIBRATE
+    
+    def is_ready_stance(self, status:str)->bool:
+        return status == LaunchRobotStatus.READY_STANCE
+    
+    def robot_start(self) -> bool:
+        # launch robot
+        if not self.pub_start_command():
+            return False
+        try:
+            timeout = 120 # 120s 等待进入校准或准备姿态
+            start_time = time.time()
+            last_print = time.time()
+            print("\033[32m>_ 机器人启动中\033[0m", end="", flush=True)
+            while True:
+                if time.time() - start_time > timeout:
+                    SDKLogger.error("Robot launch timed out")
+                    return False
+                success, status = self.get_robot_launch_status()
+                # 如果机器人启动成功，已经进入校准或准备状态或者launched状态，则启动成功
+                if success and (self.is_launched(status) or self.is_launching(status)):
+                    print("\n")
+                    return True
+                
+                current_time = time.time()
+                if current_time - last_print >= 0.8:
+                    print("\033[32m.\033[0m", end="", flush=True)
+                    last_print = current_time
+                time.sleep(0.2)
+        except KeyboardInterrupt:
+            print("\n")
+            SDKLogger.info("Received keyboard interrupt, stopping robot launch")
+            return False
+    
+    def robot_stop(self)->bool:
+        """
+        停止机器人
+        """
+        try:
+            request = roslibpy.ServiceRequest({})
+            response = self._srv_robot_stop.call(request)
+            return response.get('success', False)
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling stop robot service: {e}")
+            return False
+
+    def robot_stand(self)->bool:
+        try:
+            while True:
+                # 获取当前启动状态, 如果已启动则返回
+                success, status = self.get_robot_launch_status()
+                if not success:
+                    SDKLogger.error(f"[Error] Failed to get robot launch status: {status}")
+                    return False
+                if self.is_launched(status):
+                    return True
+                
+                # 从当前状态确认下一个状态
+                next_state = None
+                if self.is_calibrate(status):
+                    print("\033[32m>_ [校准状态]: 机器人当前处于校准状态，确认无误，请输入 y 键进入准备姿态\033[0m")
+                    next_state = LaunchRobotStatus.READY_STANCE
+                elif self.is_ready_stance(status):
+                    print("\033[32m>_ [准备姿态]: 请等待机器人缩腿到下蹲姿势，并将其下降到距离地面 2cm 处，确认无误后，请输入 y 键\033[0m")
+                    next_state = LaunchRobotStatus.LAUNCHED
+
+                if self.is_calibrate(status) or self.is_ready_stance(status):
+                    while True:
+                        try:
+                            choice = input().lower()
+                            if choice == 'y':
+                                break
+                            elif choice == 'n':
+                                return False
+                            else:
+                                SDKLogger.error("输入错误，请输入 y 键")
+                        except KeyboardInterrupt:
+                            return False
+                    
+                    # 发布站立命令
+                    if not self.pub_stand_command():
+                        return False
+
+                    # 等待进入下一个状态
+                    start_time = time.time()
+                    while True:
+                        if time.time() - start_time > self._stand_timeout: 
+                            break
+                        success, current_status = self.get_robot_launch_status()
+                        if not success:
+                            SDKLogger.error(f"[Error] Wait next state Failed to get robot launch status: {current_status}")
+                            return False
+                        if self.is_launched(current_status):
+                            return True
+                        elif current_status == next_state:
+                            # 成功进入下一个状态
+                            break
+                        time.sleep(0.2)
+                time.sleep(0.2)
+        except KeyboardInterrupt:
+            SDKLogger.info("Received keyboard interrupt, stopping robot stand")
+            return False
+
+    def pub_start_command(self) -> bool:
+        try:
+            request = roslibpy.ServiceRequest({})
+            response = self._srv_robot_start.call(request)
+            result = response.get('success', False)
+            return result
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling start service: {e}")
+            return False
+
+    def pub_stand_command(self) -> bool:
+        try:
+            request = {}
+            response = self._srv_robot_stand.call(request)
+            if not response.get('success', False):
+                SDKLogger.error(f"Failed to make robot stand: {response.get('message', '')}")
+            return response.get('success', False)
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling stand service: {e}")
+            return False
+        
+
+    def get_robot_launch_status(self)->Tuple[bool, str]:
+        try:
+            request = roslibpy.ServiceRequest({})
+            response = self._srv_get_robot_launch_status.call(request)
+            success = response.get('success', False)
+            status = response.get('message', 'unknown')
+            return success, status
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling get robot launch status service: {e}")
+            return False, "unknown"

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
+