kuavo-humanoid-sdk-ws 1.2.0__20250714105233-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/robot_audio.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.kuavo.core.audio import KuavoRobotAudioCore   
+
+class KuavoRobotAudio:
+    """Audio system interface for controlling audio playback functionality of Kuavo humanoid robot.
+    
+    Provides functionality to play music files.
+    """
+    
+    def __init__(self):
+        """Initialize the audio system."""
+        self.audio = KuavoRobotAudioCore()
+
+    def play_audio(self, file_name: str, volume: int = 100, speed: float = 1.0) -> bool:
+        """Play the specified audio file.
+        
+        Args:
+            file_name (str): Name of the audio file to play
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        return self.audio.play_audio(file_name, volume, speed)
+    
+    def stop_music(self):
+        """Stop the currently playing audio."""
+        return self.audio.stop_music()
+    
+    def text_to_speech(self, text: str, volume: float = 0.5) -> bool:
+        """Synthesize and play the specified text.
+        
+        Args:
+            text (str): Text to be played
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        return self.audio.text_to_speech(text, volume)

kuavo_humanoid_sdk/kuavo/robot_head.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+class KuavoRobotHead:
+    def __init__(self):
+        self._kuavo_core = KuavoRobotCore()
+    
+    def control_head(self, yaw: float, pitch: float)->bool:
+        """
+            Control the head of the robot.
+            Args:
+                yaw (float): The yaw angle of the head in radians, range [-1.396, 1.396] (-80 to 80 degrees).
+                pitch (float): The pitch angle of the head in radians, range [-0.436, 0.436] (-25 to 25 degrees).
+            Returns:
+                bool: True if the head is controlled successfully, False otherwise.
+        """
+        # Check yaw limits (-80 to 80 degrees)
+        if yaw < -math.pi*4/9 or yaw > math.pi*4/9:  # -80 to 80 degrees in radians
+            SDKLogger.warn(f"[Robot] yaw {yaw} exceeds limit [-{math.pi*4/9:.3f}, {math.pi*4/9:.3f}] radians (-80 to 80 degrees), will be limited")
+        limited_yaw = min(math.pi*4/9, max(-math.pi*4/9, yaw))
+
+        # Check pitch limits (-25 to 25 degrees)
+        if pitch < -math.pi/7.2 or pitch > math.pi/7.2:  # -25 to 25 degrees in radians
+            SDKLogger.warn(f"[Robot] pitch {pitch} exceeds limit [-{math.pi/7.2:.3f}, {math.pi/7.2:.3f}] radians (-25 to 25 degrees), will be limited")
+        limited_pitch = min(math.pi/7.2, max(-math.pi/7.2, pitch))
+        return self._kuavo_core.control_robot_head(yaw=limited_yaw, pitch=limited_pitch)
+
+    def enable_head_tracking(self, target_id: int)->bool:
+        """Enable the head tracking.
+        """
+        return self._kuavo_core.enable_head_tracking(target_id)
+    
+    def disable_head_tracking(self)->bool:
+        """Disable the head tracking.
+        """
+        return self._kuavo_core.disable_head_tracking()

kuavo_humanoid_sdk/kuavo/robot_info.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from typing import Tuple
+from kuavo_humanoid_sdk.interfaces.robot_info import RobotInfoBase
+from kuavo_humanoid_sdk.kuavo.core.ros.param import RosParamWebsocket, make_robot_param
+
+class KuavoRobotInfo(RobotInfoBase):
+    def __init__(self, robot_type: str = "kuavo"):
+        super().__init__(robot_type=robot_type)
+        
+        # Load robot parameters from ROS parameter server
+        kuavo_ros_param = make_robot_param()
+        self._ros_param = RosParamWebsocket()
+            
+        self._robot_version = kuavo_ros_param['robot_version']
+        self._end_effector_type = kuavo_ros_param['end_effector_type']
+        self._arm_joint_dof = kuavo_ros_param['arm_dof']
+        self._joint_dof = kuavo_ros_param['arm_dof'] + kuavo_ros_param['leg_dof'] + kuavo_ros_param['head_dof']
+        self._joint_names = kuavo_ros_param['joint_names']
+        self._end_frames_names = kuavo_ros_param['end_frames_names']
+        self._head_joint_dof = kuavo_ros_param['head_dof']
+        self._head_joint_names = self._joint_names[-2:]
+        self._arm_joint_names = self._joint_names[12:self._arm_joint_dof + 12]
+    @property
+    def robot_version(self) -> str:
+        """Return the version of the robot.
+
+        Returns:
+            str: The robot version, e.g. "42", "43"...
+        """
+        return self._robot_version
+
+    @property
+    def end_effector_type(self) -> str:
+        """Return the type of the end effector.
+
+        Returns:
+            str: The end effector type, where:
+                - "qiangnao" means "dexteroushand"
+                - "lejuclaw" means "lejuclaw"
+                - "qiangnao_touch" means "touchdexteroushand"
+                - ...
+        """
+        return self._end_effector_type
+
+    @property
+    def joint_names(self) -> list:
+        """Return the names of all joints in the robot.
+
+        Returns:
+            list: A list containing the names of all robot joints.
+        """
+        return self._joint_names
+
+    @property
+    def joint_dof(self) -> int:
+        """Return the total number of joints in the robot.
+
+        Returns:
+            int: Total number of joints, e.g. 28
+        """
+        return self._joint_dof
+
+    @property
+    def arm_joint_dof(self) -> int:
+        """Return the number of joints in the double-arm.
+
+        Returns:
+            int: Number of joints in double-arm, e.g. 14
+        """
+        return self._arm_joint_dof
+
+    @property
+    def arm_joint_names(self) -> list:
+        """Return the names of joints in the double-arm.
+
+        Returns:
+            list: A list containing the names of joints in the double-arm.
+        """
+        return self._arm_joint_names
+
+    @property
+    def head_joint_dof(self) -> int:
+        """Return the number of joints in the head.
+
+        Returns:
+            int: Number of joints in head, e.g. 2
+        """
+        return self._head_joint_dof
+
+    @property
+    def head_joint_names(self) -> list:
+        """Return the names of joints in the head.
+
+        Returns:
+            list: A list containing the names of joints in the head.
+        """
+        return self._head_joint_names
+
+    @property
+    def eef_frame_names(self) -> Tuple[str, str]:
+        """Returns the names of the end effector frames.
+
+        Returns:
+            Tuple[str, str]:
+                A tuple containing the end effector frame names, where:
+                - First element is the left hand frame name
+                - Second element is the right hand frame name
+                e.g. ("zarm_l7_link", "zarm_r7_link")
+        """
+        return self._end_frames_names[1], self._end_frames_names[2]
+    
+    def __str__(self) -> str:
+        return f"KuavoRobotInfo(robot_type={self.robot_type}, robot_version={self.robot_version}, end_effector_type={self.end_effector_type}, joint_names={self.joint_names}, joint_dof={self.joint_dof}, arm_joint_dof={self.arm_joint_dof})"

kuavo_humanoid_sdk/kuavo/robot_observation.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.interfaces.data_types import (
+    KuavoJointCommand, KuavoTwist)
+from kuavo_humanoid_sdk.kuavo.core.ros.observation import KuavoRobotObservationCoreWebsocket
+
+class KuavoRobotObservation:
+    """Class for accessing robot observation data.
+    
+    This class provides a high-level interface to access robot observation data,
+    including joint commands, velocity commands, and pose commands.
+    
+    Attributes:
+        _obs_core: The core observation object that handles ROS communication.
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """Initialize the robot observation interface.
+        
+        Args:
+            robot_type (str): Type of the robot. Defaults to "kuavo".
+        """
+        self._obs_core = KuavoRobotObservationCoreWebsocket()
+
+    @property
+    def joint_command(self) -> KuavoJointCommand:
+        """Get the current joint command.
+        
+        Returns:
+            KuavoJointCommand: Object containing position, velocity, and torque commands
+                for all robot joints.
+        """
+        return self._obs_core.joint_command
+
+    @property
+    def cmd_vel(self) -> KuavoTwist:
+        """Get the current velocity command.
+        
+        Returns:
+            KuavoTwist: Object containing linear velocity (m/s) and angular velocity (rad/s) commands.
+        """
+        return self._obs_core.cmd_vel
+
+    @property
+    def cmd_pose(self) -> KuavoTwist:
+        """Get the current pose command.
+        
+        Returns:
+            KuavoTwist: Object containing linear pose commands (m) and angular pose commands (rad).
+        """
+        return self._obs_core.cmd_pose
+
+    @property    
+    def arm_position_command(self) -> list:
+        """Get the position commands for the arm joints.
+        
+        Returns:
+            list: Position commands for arm joints (indices 12-25) in radians.
+        """
+        return self._obs_core.arm_position_command
+
+    @property
+    def head_position_command(self) -> list:
+        """Get the position commands for the head joints.
+        
+        Returns:
+            list: Position commands for head joints (indices 26-27) in radians.
+        """
+        return self._obs_core.head_position_command

kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -0,0 +1,303 @@
+#!/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 KuavoRobotStateCoreWebsocket
+
+class KuavoRobotState:
+    def __init__(self, robot_type: str = "kuavo"):
+        self._rs_core = KuavoRobotStateCoreWebsocket()
+
+    @property
+    def imu_data(self) -> KuavoImuData:
+        """Get Robot IMU Data.
+
+        Gets the current IMU sensor data from the robot, including gyroscope, accelerometer,
+        free acceleration and orientation quaternion measurements.
+
+        Returns:
+            :class:`KuavoImuData`: IMU data containing:
+                * gyro (:obj:`tuple` of :obj:`float`): Gyroscope measurements (x, y, z) in rad/s
+                * acc (:obj:`tuple` of :obj:`float`): Accelerometer measurements (x, y, z) in m/s^2
+                * free_acc (:obj:`tuple` of :obj:`float`): Free acceleration (x, y, z) in m/s^2
+                * quat (:obj:`tuple` of :obj:`float`): Orientation quaternion (x, y, z, w)
+        """
+        return self._rs_core.imu_data
+    
+    @property
+    def joint_state(self) -> KuavoJointData:
+        """Get Robot Joint Data.
+
+        Get Robot Joint Data, including joint positions, velocities, torques and accelerations.
+
+        The data includes:
+            - Joint positions (angles) in radians
+            - Joint velocities in radians/second 
+            - Joint torques/efforts in Newton-meters, A
+            - Joint acceleration
+
+        Returns:
+            KuavoJointData: A dictionary containing joint state data with the following keys:
+                position (list[float]): Joint positions, length = joint_dof(28)
+                velocity (list[float]): Joint velocities, length = joint_dof(28)  
+                torque (list[float]): Joint torques, length = joint_dof(28)
+                acceleration (list[float]): Joint accelerations, length = joint_dof(28)
+        """
+        return self._rs_core.joint_data
+    
+    @property
+    def com_height(self)->float:
+        """Get the height of the robot's center of mass.
+
+        Returns:
+            float: The height of the robot's center of mass in meters.
+        """
+        return self._rs_core.com_height
+    
+    @property
+    def odometry(self) -> KuavoOdometry:
+        """Get Robot Odometry Data.
+
+        Gets the current odometry data from the robot, including position, orientation,
+        linear velocity and angular velocity measurements.
+
+        Returns:
+            KuavoOdometry: A dictionary containing odometry data with the following keys:
+                position (tuple): Position (x, y, z) in meters
+                orientation (tuple): Orientation as quaternion (x, y, z, w)
+                linear (tuple): Linear velocity (x, y, z) in m/s
+                angular (tuple): Angular velocity (x, y, z) in rad/s
+        """
+        return self._rs_core.odom_data   
+
+    
+    def robot_position(self) -> Tuple[float, float, float]:
+        """Returns the robot's position in world coordinates.
+
+        Returns:
+            Tuple[float, float, float]: Position (x, y, z) in meters.
+        """
+        return tuple(self._rs_core.odom_data.position)
+
+    def robot_orientation(self) -> Tuple[float, float, float, float]:
+        """Returns the robot's orientation in world coordinates.
+
+        Returns:
+            Tuple[float, float, float, float]: Orientation as quaternion (x, y, z, w).
+        """
+        return tuple(self._rs_core.odom_data.orientation)
+
+    def linear_velocity(self) -> Tuple[float, float, float]:
+        """Returns the robot's linear velocity in world coordinates.
+
+        Returns:
+            Tuple[float, float, float]: Linear velocity (x, y, z) in m/s.
+        """
+        return tuple(self._rs_core.odom_data.linear)
+    
+
+    def angular_velocity(self) -> Tuple[float, float, float]:
+        """Returns the robot's angular velocity in world coordinates.
+
+        Returns:
+            Tuple[float, float, float]: Angular velocity (x, y, z).
+        """
+        return tuple(self._rs_core.odom_data.angular)
+
+    def arm_joint_state(self) -> KuavoJointData:
+        """Get the current state of the robot arm joints.
+
+        Get the current state of the robot arm joints, including:
+            - Joint positions (angles) in radians
+            - Joint velocities in radians/second 
+            - Joint torques/efforts in Newton-meters, A
+            - Joint acceleration
+
+        Returns:
+            KuavoJointData: Arm joint data containing:
+                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:
+        """Get the current control mode of the robot arm.
+
+        Returns:
+            KuavoArmCtrlMode: Current arm control mode:
+                ArmFixed: 0 - The robot arm is in a fixed position.
+                AutoSwing: 1 - The robot arm is in automatic swing mode.
+                ExternalControl: 2 - The robot arm is controlled externally.
+                or None.
+        """
+        return KuavoArmCtrlMode(self._rs_core.arm_control_mode)
+    
+    def manipulation_mpc_ctrl_mode(self) -> KuavoManipulationMpcCtrlMode:
+        """Get the current control mode of the robot manipulation MPC.
+
+        Returns:
+            KuavoManipulationMpcCtrlMode: Current manipulation MPC control mode.
+
+        """
+        return self._rs_core.manipulation_mpc_ctrl_mode
+    
+    def manipulation_mpc_control_flow(self) -> KuavoManipulationMpcControlFlow:
+        """Get the current control flow of the robot manipulation.
+
+        Returns:
+            KuavoManipulationMpcControlFlow: Current manipulation control flow.
+        """
+        return self._rs_core.manipulation_mpc_control_flow
+    
+    def manipulation_mpc_frame(self) -> KuavoManipulationMpcFrame:
+        """Get the current frame of the robot manipulation MPC.
+
+        Returns:
+            KuavoManipulationMpcFrame: Current manipulation MPC frame.
+        """
+        return self._rs_core.manipulation_mpc_frame
+    
+    def head_joint_state(self) -> KuavoJointData:
+        """Get the current state of the robot head joints.
+
+        Gets the current state data for the robot's head joints, including position,
+        velocity, torque and acceleration values.
+
+        Returns:
+            KuavoJointData: A data structure containing the head joint states:
+                position (list[float]): Joint positions in radians, length=head_dof(2)
+                velocity (list[float]): Joint velocities in rad/s, length=head_dof(2) 
+                torque (list[float]): Joint torques in Nm, length=head_dof(2)
+                acceleration (list[float]): Joint accelerations in rad/s^2, length=head_dof(2)
+                
+            The joint order is [yaw, pitch].
+        """
+        # 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]:
+        """Get the current state of the robot's end effectors.
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: A tuple containing the state of the left and right end effectors.
+                Each EndEffectorState contains:
+                    - position: (float, float, float) - XYZ position in meters
+                    - orientation: (float, float, float, float) - Quaternion orientation
+                    - state: EndEffectorState.GraspingState - Current grasping state (UNKNOWN, OPEN, CLOSED)
+        """
+        return copy.deepcopy(self._rs_core.eef_state)
+
+    def gait_name(self)->str:
+        """Get the current gait name of the robot.
+
+        Returns:
+            str: The name of the current gait, e.g. 'trot', 'walk', 'stance', 'custom_gait'.
+        """
+        return self._rs_core.gait_name()
+    
+
+    def is_stance(self) -> bool:
+        """Check if the robot is currently in stance mode.
+
+        Returns:
+            bool: True if robot is in stance mode, False otherwise.
+        """
+        return self._rs_core.is_gait('stance')
+
+    def is_walk(self) -> bool:
+        """Check if the robot is currently in walk mode.
+
+        Returns:
+            bool: True if robot is in walk mode, False otherwise.
+        """
+        return self._rs_core.is_gait('walk')
+
+    def is_step_control(self) -> bool:
+        """Check if the robot is currently in step control mode.
+
+        Returns:
+            bool: True if robot is in step control mode, False otherwise.
+        """
+        return self._rs_core.is_gait('custom_gait')
+
+    def wait_for_stance(self, timeout:float=5.0)->bool:
+        """Wait for the robot to enter stance state.
+
+        Args:
+            timeout (float): The maximum time to wait for the robot to enter stance state in seconds.
+
+        Returns:
+            bool: True if the robot enters stance state within the specified timeout, False otherwise.
+        """
+        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:
+        """Wait for the robot to enter trot state.
+
+        Args:
+            timeout (float): The maximum time to wait for the robot to enter trot state in seconds.
+
+        Returns:
+            bool: True if the robot enters trot state within the specified timeout, False otherwise.
+        """
+        return self.wait_for_walk(timeout=timeout)
+
+    def wait_for_walk(self, timeout:float=5.0)->bool:
+        """Wait for the robot to enter walk state.
+
+        Args:
+            timeout (float): The maximum time to wait for the robot to enter walk state in seconds.
+
+        Returns:
+            bool: True if the robot enters walk state within the specified timeout, False otherwise.
+        """
+        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:
+        """Wait for the robot to enter step control state.
+
+        Args:
+            timeout (float): The maximum time to wait for the robot to enter step control state in seconds.
+
+        Returns:
+            bool: True if the robot enters step control state within the specified timeout, False otherwise.
+        """
+        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,58 @@
+#!/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)
+from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCoreWebsocket
+from typing import Union
+
+class KuavoRobotTools:
+    """机器人工具类,提供坐标系转换接口。
+    
+    该类封装了不同机器人坐标系之间的坐标变换查询功能,支持多种返回数据格式。
+    """
+    
+    def __init__(self):
+        self.tools_core = KuavoRobotToolsCoreWebsocket()
+
+    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)