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

kuavo_humanoid_sdk/kuavo/robot_arm.py

@@ -0,0 +1,248 @@
+#!/usr/bin/env python3
+# coding: utf-8
+
+import math
+from typing import Tuple
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
+
+class KuavoRobotArm:
+    """Kuavo机器人手臂控制类。
+    
+    提供了控制机器人手臂的各种接口,包括关节位置控制、轨迹控制、末端执行器姿态控制等。
+    """
+    def __init__(self):
+        self._kuavo_core = KuavoRobotCore()
+        self._robot_info = KuavoRobotInfo(robot_type="kuavo")
+    
+    def arm_reset(self)-> bool:
+        """重置机器人手臂。
+        
+        Returns:
+            bool: 重置成功返回True,否则返回False。
+        """
+        return self._kuavo_core.robot_arm_reset()
+    
+    def manipulation_mpc_reset(self)-> bool:
+        """重置机器人 Manipulation MPC 控制器。
+        
+        Returns:
+            bool: 重置成功返回True,否则返回False。
+        """
+        return self._kuavo_core.robot_manipulation_mpc_reset()
+        
+    def control_arm_joint_positions(self, joint_position:list)->bool:
+        """控制机器人手臂关节位置。
+
+        Args:
+            joint_position (list): 关节位置列表,单位为弧度
+
+        Raises:
+            ValueError: 如果关节位置列表长度不正确
+            ValueError: 如果关节位置超出[-π, π]范围
+            RuntimeError: 如果在控制手臂时机器人不在站立状态
+
+        Returns:
+            bool: 控制成功返回True,否则返回False
+        """
+        if len(joint_position) != self._robot_info.arm_joint_dof:
+            raise ValueError("Invalid position length. Expected {}, got {}".format(self._robot_info.arm_joint_dof, len(joint_position)))
+        
+        # Check if joint positions are within ±180 degrees (±π radians)
+        for pos in joint_position:
+            if abs(pos) > math.pi:
+                raise ValueError(f"Joint position {pos} rad exceeds ±π rad (±180 deg) limit")
+
+        return self._kuavo_core.control_robot_arm_joint_positions(joint_data=joint_position)
+
+    def control_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
+        """控制机器人手臂关节轨迹。
+
+        Args:
+            times (list): 时间间隔列表,单位为秒
+            joint_q (list): 关节位置列表,单位为弧度
+
+        Raises:
+            ValueError: 如果times列表长度不正确
+            ValueError: 如果关节位置列表长度不正确
+            ValueError: 如果关节位置超出[-π, π]范围
+            RuntimeError: 如果在控制手臂时机器人不在站立状态
+
+        Returns:
+            bool: 控制成功返回True,否则返回False
+        """
+        if len(times) != len(joint_q):
+            raise ValueError("Invalid input. times and joint_q must have thesame length.")
+        
+        # Check if joint positions are within ±180 degrees (±π radians)
+        q_degs = []
+        for q in joint_q:
+            if any(abs(pos) > math.pi for pos in q):
+                raise ValueError("Joint positions must be within ±π rad (±180 deg)")
+            if len(q) != self._robot_info.arm_joint_dof:
+                raise ValueError("Invalid position length. Expected {}, got {}".format(self._robot_info.arm_joint_dof, len(q)))
+            # Convert joint positions from radians to degrees
+            q_degs.append([(p * 180.0 / math.pi) for p in q])
+
+        return self._kuavo_core.control_robot_arm_joint_trajectory(times=times, joint_q=q_degs)
+
+    def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:
+        """控制机器人末端执行器姿态。
+
+        Args:
+            left_pose (KuavoPose): 左手臂姿态,包含xyz位置和四元数方向
+            right_pose (KuavoPose): 右手臂姿态,包含xyz位置和四元数方向
+            frame (KuavoManipulationMpcFrame): 末端执行器姿态的坐标系
+
+        Returns:
+            bool: 控制成功返回True,否则返回False
+        """
+        return self._kuavo_core.control_robot_end_effector_pose(left_pose, right_pose, frame)
+
+    def set_fixed_arm_mode(self) -> bool:
+        """固定/冻结机器人手臂。
+
+        Returns:
+            bool: 固定/冻结成功返回True,否则返回False
+        """
+        return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ArmFixed)
+
+    def set_auto_swing_arm_mode(self) -> bool:
+        """设置手臂自动摆动模式。
+
+        Returns:
+            bool: 设置成功返回True,否则返回False
+        """
+        return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.AutoSwing)
+    
+    def set_external_control_arm_mode(self) -> bool:
+        """设置手臂外部控制模式。
+
+        Returns:
+            bool: 设置成功返回True,否则返回False
+        """
+        return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl)
+
+    def set_manipulation_mpc_mode(self, ctrl_mode: KuavoManipulationMpcCtrlMode) -> bool:
+        """设置 Manipulation MPC 控制模式。
+
+        Returns:
+            bool: 设置成功返回True,否则返回False
+        """
+        return self._kuavo_core.change_manipulation_mpc_ctrl_mode(ctrl_mode)
+    
+    def set_manipulation_mpc_control_flow(self, control_flow: KuavoManipulationMpcControlFlow) -> bool:
+        """设置 Manipulation MPC 控制流。
+
+        Returns:
+            bool: 设置成功返回True,否则返回False
+        """
+        return self._kuavo_core.change_manipulation_mpc_control_flow(control_flow)
+    
+    def set_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame) -> bool:
+        """设置 Manipulation MPC 坐标系。
+
+        Returns:
+            bool: 设置成功返回True,否则返回False
+        """
+        return self._kuavo_core.change_manipulation_mpc_frame(frame)
+    
+    """ 手臂正向运动学和逆向运动学 """
+    def arm_ik(self, 
+               left_pose: KuavoPose, 
+               right_pose: KuavoPose,
+               left_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
+               right_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
+               arm_q0: list = None,
+               params: KuavoIKParams=None) -> list:
+        """机器人手臂逆向运动学求解
+        
+        Args:
+            left_pose (KuavoPose): 左手臂目标姿态,包含xyz位置和四元数方向
+            right_pose (KuavoPose): 右手臂目标姿态,包含xyz位置和四元数方向
+            left_elbow_pos_xyz (list): 左肘部位置。如果为[0.0, 0.0, 0.0],则忽略
+            right_elbow_pos_xyz (list): 右肘部位置。如果为[0.0, 0.0, 0.0],则忽略
+            arm_q0 (list, optional): 初始关节位置,单位为弧度。如果为None,则忽略
+            params (KuavoIKParams, optional): 逆向运动学参数。如果为None,则忽略，包含:
+                - major_optimality_tol: 主要最优性容差 \n
+                - major_feasibility_tol: 主要可行性容差 \n
+                - minor_feasibility_tol: 次要可行性容差 \n
+                - major_iterations_limit: 主要迭代次数限制 \n
+                - oritation_constraint_tol: 方向约束容差 \n
+                - pos_constraint_tol: 位置约束容差,当pos_cost_weight==0.0时生效 \n
+                - pos_cost_weight: 位置代价权重。设为0.0可获得高精度 \n
+                
+        Returns:
+            list: 关节位置列表,单位为弧度。如果计算失败返回None
+
+        Warning:
+            此函数需要在初始化SDK时设置 :attr:`KuavoSDK.Options.WithIK` 选项。
+        """
+        return self._kuavo_core.arm_ik(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
+
+    def arm_fk(self, q: list) -> Tuple[KuavoPose, KuavoPose]:
+        """机器人手臂正向运动学求解
+        
+        Args:
+            q (list): 关节位置列表,单位为弧度
+            
+        Returns:
+            Tuple[KuavoPose, KuavoPose]: 左右手臂姿态的元组,
+                如果计算失败返回(None, None)
+        
+        Warning:
+            此函数需要在初始化SDK时设置 :attr:`KuavoSDK.Options.WithIK` 选项。
+        """
+        if len(q) != self._robot_info.arm_joint_dof:
+            raise ValueError("Invalid position length. Expected {}, got {}".format(self._robot_info.arm_joint_dof, len(q)))
+        
+        result = self._kuavo_core.arm_fk(q)
+        if result is None:
+            return None, None
+        return result
+
+    def control_hand_wrench(self, left_wrench: list, right_wrench: list) -> bool:
+        """控制机器人末端力/力矩
+        
+        Args:
+            left_wrench (list): 左手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+            right_wrench (list): 右手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
+                单位:
+                Fx,Fy,Fz: 牛顿(N)
+                Tx,Ty,Tz: 牛·米(N·m)
+        
+        Returns:
+            bool: 控制成功返回True, 否则返回False
+        """
+        return self._kuavo_core.control_hand_wrench(left_wrench, right_wrench)
+
+    def is_arm_collision(self)->bool:
+        """判断当前是否发生碰撞
+        
+        Returns:
+            bool: 发生碰撞返回True,否则返回False
+        """
+        return self._kuavo_core.is_arm_collision()
+    
+    def release_arm_collision_mode(self):
+        """释放碰撞模式
+        """
+        self._kuavo_core.release_arm_collision_mode()
+
+    def wait_arm_collision_complete(self):
+        """等待碰撞完成
+        """
+        self._kuavo_core.wait_arm_collision_complete()
+
+    def set_arm_collision_mode(self, enable: bool):
+        """设置碰撞模式
+        """
+        self._kuavo_core.set_arm_collision_mode(enable)
+
+# if __name__ == "__main__":
+#     arm = KuavoRobotArm()
+#     arm.set_manipulation_mpc_mode(KuavoManipulationMpcCtrlMode.ArmOnly)
+#     arm.set_manipulation_mpc_control_flow(KuavoManipulationMpcControlFlow.DirectToWbc)
+#     arm.set_manipulation_mpc_frame(KuavoManipulationMpcFrame.WorldFrame)
+#     arm.control_robot_end_effector_pose(KuavoPose(position=[0.3, 0.4, 0.9], orientation=[0.0, 0.0, 0.0, 1.0]), KuavoPose(position=[0.3, -0.5, 1.0], orientation=[0.0, 0.0, 0.0, 1.0]), KuavoManipulationMpcFrame.WorldFrame)

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:
+    """Kuavo 机器人音频系统接口，用于控制音频播放功能。
+    
+    提供音乐文件播放功能。
+    """
+    
+    def __init__(self):
+        """初始化音频系统。"""
+        self.audio = KuavoRobotAudioCore()
+
+    def play_audio(self, file_name: str, volume: int = 100, speed: float = 1.0) -> bool:
+        """播放指定的音频文件。
+        
+        Args:
+            file_name (str): 要播放的音频文件名
+                
+        Returns:
+            bool: 如果播放请求成功发送返回True，否则返回False
+        """
+        return self.audio.play_audio(file_name, volume, speed)
+    
+    def stop_music(self):
+        """停止当前正在播放的音频。"""
+        return self.audio.stop_music()
+    
+    def text_to_speech(self, text: str, volume: float = 0.5) -> bool:
+        """将指定文本合成并播放。
+        
+        Args:
+            text (str): 要播放的文本
+                
+        Returns:
+            bool: 如果播放请求成功发送返回True，否则返回False
+        """
+        return self.audio.text_to_speech(text, volume)

kuavo_humanoid_sdk/kuavo/robot_head.py

@@ -0,0 +1,50 @@
+#!/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:
+        """控制机器人头部。
+
+        Args:
+            yaw (float): 头部偏航角，单位为弧度，范围 [-1.396, 1.396] (-80度 到 80度)。
+            pitch (float): 头部俯仰角，单位为弧度，范围 [-0.436, 0.436] (-25度 到 25度)。
+
+        Returns:
+            bool: 如果控制成功返回True，否则返回False。
+        """
+        # 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 - 0.001 or pitch > math.pi/7.2 + 0.001:  # -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:
+        """启用头部跟踪功能，在机器人运动过程中，头部将始终追踪指定的 Apriltag ID
+
+        Args:
+            target_id (int): 目标ID。
+
+        Returns:
+            bool: 如果启用成功返回True，否则返回False。
+        """
+        return self._kuavo_core.enable_head_tracking(target_id)
+    
+    def disable_head_tracking(self)->bool:
+        """禁用头部跟踪功能。
+
+        Returns:
+            bool: 如果禁用成功返回True，否则返回False。
+        """
+        return self._kuavo_core.disable_head_tracking()

kuavo_humanoid_sdk/kuavo/robot_info.py

@@ -0,0 +1,113 @@
+#!/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 RosParameter, 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 = RosParameter()
+            
+        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:
+        """返回 Kuavo 机器人的版本。
+
+        Returns:
+            str: 机器人版本号，例如 "42"、"45" 等。
+        """
+        return self._robot_version
+
+    @property
+    def end_effector_type(self) -> str:
+        """返回 Kuavo 机器人末端执行器的类型。
+
+        Returns:
+            str: 末端执行器类型，其中：
+                - ``qiangnao`` 表示普通灵巧手
+                - ``lejuclaw`` 表示乐聚二指夹爪
+                - ``qiangnao_touch`` 表示触觉灵巧手
+                - ...
+        """
+        return self._end_effector_type
+
+    @property
+    def joint_names(self) -> list:
+        """返回 Kuavo 机器人所有关节的名称。
+
+        Returns:
+            list: 包含所有关节名称的列表。
+        """
+        return self._joint_names
+
+    @property
+    def joint_dof(self) -> int:
+        """返回 Kuavo 机器人的总关节数。
+
+        Returns:
+            int: 总关节数，例如 28。
+        """
+        return self._joint_dof
+
+    @property
+    def arm_joint_dof(self) -> int:
+        """返回 Kuavo 机器人双臂的关节数。
+
+        Returns:
+            int: 双臂的关节数，例如 14。 
+        """
+        return self._arm_joint_dof
+
+    @property
+    def arm_joint_names(self) -> list:
+        """返回 Kuavo 机器人双臂关节的名称。
+
+        Returns:
+            list: 包含双臂关节名称的列表。
+        """
+        return self._arm_joint_names
+
+    @property
+    def head_joint_dof(self) -> int:
+        """返回 Kuavo 机器人头部的关节数。
+
+        Returns:
+            int: 头部的关节数，例如 2。
+        """
+        return self._head_joint_dof
+
+    @property
+    def head_joint_names(self) -> list:
+        """返回 Kuavo 机器人头部关节的名称。
+
+        Returns:
+            list: 包含头部关节名称的列表。
+        """
+        return self._head_joint_names
+
+    @property
+    def eef_frame_names(self) -> Tuple[str, str]:
+        """返回 Kuavo 机器人末端执行器坐标系的名称。
+
+        Returns:
+            Tuple[str, str]: 包含末端执行器坐标系名称的元组，其中：\n
+                - 第一个元素是左手坐标系名称\n
+                - 第二个元素是右手坐标系名称\n
+                例如 ("zarm_l7_link", "zarm_r7_link") \n
+        """
+        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,64 @@
+#!/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 KuavoRobotObservationCore
+
+class KuavoRobotObservation:
+    """用于访问机器人观测数据的类。
+    
+    该类提供了一个高级接口来访问机器人的观测数据，包括关节命令、速度命令和姿态命令。
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """初始化机器人观测接口
+        
+        Args:
+            robot_type (str): 机器人类型。默认为"kuavo"
+        """
+        self._obs_core = KuavoRobotObservationCore()
+
+    @property
+    def joint_command(self) -> KuavoJointCommand:
+        """获取当前关节控制命令
+        
+        Returns:
+            KuavoJointCommand: 包含所有机器人关节的位置、速度和力矩命令的对象。
+        """
+        return self._obs_core.joint_command
+
+    @property
+    def cmd_vel(self) -> KuavoTwist:
+        """获取当前 cmd_vel 速度控制命令
+        
+        Returns:
+            KuavoTwist: 包含线速度(m/s)和角速度(rad/s)命令的对象。
+        """
+        return self._obs_core.cmd_vel
+
+    @property
+    def cmd_pose(self) -> KuavoTwist:
+        """获取当前 cmd_pose 姿态控制命令
+        
+        Returns:
+            KuavoTwist: 包含线性姿态命令(m)和角度姿态命令(rad)的对象。
+        """
+        return self._obs_core.cmd_pose
+
+    @property    
+    def arm_position_command(self) -> list:
+        """获取手臂关节的位置控制命令
+        
+        Returns:
+            list: 手臂关节(索引12-25)的位置命令，单位为弧度。
+        """
+        return self._obs_core.arm_position_command
+
+    @property
+    def head_position_command(self) -> list:
+        """获取头部关节的位置控制命令
+        
+        Returns:
+            list: 头部关节(索引26-27)的位置命令，单位为弧度。
+        """
+        return self._obs_core.head_position_command