kuavo-humanoid-sdk 1.2.0__20250714105223-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:
+    """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 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:
+        """启用头部跟踪功能，在机器人运动过程中，头部将始终追踪指定的 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

kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -0,0 +1,299 @@
+#!/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 KuavoRobotStateCore
+
+class KuavoRobotState:
+    def __init__(self, robot_type: str = "kuavo"):
+        self._rs_core = KuavoRobotStateCore()
+
+    @property
+    def imu_data(self) -> KuavoImuData:
+        """获取 Kuavo 机器人IMU数据。
+
+        获取机器人当前的 IMU 传感器数据，包括陀螺仪、加速度计、自由加速度和方向四元数测量值。
+
+        Returns:
+            :class:`KuavoImuData`: IMU数据包含:
+                * gyro (:obj:`tuple` of :obj:`float`): 陀螺仪测量值 (x, y, z)，单位rad/s
+                * acc (:obj:`tuple` of :obj:`float`): 加速度计测量值 (x, y, z)，单位m/s^2
+                * free_acc (:obj:`tuple` of :obj:`float`): 自由加速度 (x, y, z)，单位m/s^2
+                * quat (:obj:`tuple` of :obj:`float`): 方向四元数 (x, y, z, w)
+        """
+        return self._rs_core.imu_data
+    
+    @property
+    def joint_state(self) -> KuavoJointData:
+        """获取 Kuavo 机器人关节数据。
+
+        获取机器人关节数据，包括关节位置、速度、扭矩和加速度。
+
+        数据包括:
+            - 关节位置(角度)，单位为弧度
+            - 关节速度，单位为弧度/秒
+            - 关节扭矩/力矩，单位为牛顿米、安培
+            - 关节加速度
+
+        Returns:
+            KuavoJointData: 包含以下关节状态数据的字典:
+                * position (list[float]): 关节位置，长度 = joint_dof(28)
+                * velocity (list[float]): 关节速度，长度 = joint_dof(28)
+                * torque (list[float]): 关节扭矩，长度 = joint_dof(28)
+                * acceleration (list[float]): 关节加速度，长度 = joint_dof(28)
+        """
+        return self._rs_core.joint_data
+    
+    @property
+    def com_height(self)->float:
+        """获取机器人质心高度。
+
+        Returns:
+            float: 机器人质心高度，单位为米。
+        """
+        return self._rs_core.com_height
+    
+    @property
+    def odometry(self) -> KuavoOdometry:
+        """获取 Kuavo 机器人里程计数据。
+
+        获取机器人当前的里程计数据，包括位置、方向、线速度和角速度测量值。
+
+        Returns:
+            KuavoOdometry: 包含以下里程计数据的字典:
+                * position (tuple): 位置 (x, y, z)，单位为米
+                * orientation (tuple): 方向四元数 (x, y, z, w)
+                * linear (tuple): 线速度 (x, y, z)，单位为m/s
+                * angular (tuple): 角速度 (x, y, z)，单位为rad/s
+        """
+        return self._rs_core.odom_data   
+
+    
+    def robot_position(self) -> Tuple[float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的位置。
+
+        Returns:
+            Tuple[float, float, float]: 位置 (x, y, z)，单位为米。
+        """
+        return tuple(self._rs_core.odom_data.position)
+
+    def robot_orientation(self) -> Tuple[float, float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的方向。
+
+        Returns:
+            Tuple[float, float, float, float]: 方向四元数 (x, y, z, w)。
+        """
+        return tuple(self._rs_core.odom_data.orientation)
+
+    def linear_velocity(self) -> Tuple[float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的线速度。
+
+        Returns:
+            Tuple[float, float, float]: 线速度 (x, y, z)，单位为m/s。
+        """
+        return tuple(self._rs_core.odom_data.linear)
+    
+
+    def angular_velocity(self) -> Tuple[float, float, float]:
+        """返回 Kuavo 机器人在世界坐标系中的角速度。
+
+        Returns:
+            Tuple[float, float, float]: 角速度 (x, y, z)。
+        """
+        return tuple(self._rs_core.odom_data.angular)
+
+    def arm_joint_state(self) -> KuavoJointData:
+        """获取 Kuavo 机器人手臂关节的当前状态。
+
+        获取 Kuavo 机器人手臂关节的当前状态，包括:
+            - 关节位置(角度)，单位为弧度
+            - 关节速度，单位为弧度/秒
+            - 关节扭矩/力矩，单位为牛顿米、安培
+            - 关节加速度
+
+        Returns:
+            KuavoJointData: 手臂关节数据包含:
+                * 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:
+        """获取 Kuavo 机器人手臂的当前控制模式。
+
+        Returns:
+            KuavoArmCtrlMode: 当前手臂控制模式:
+                * ArmFixed: 0 - 机器人手臂处于固定位置。
+                * AutoSwing: 1 - 机器人手臂处于自动摆动模式。
+                * ExternalControl: 2 - 机器人手臂由外部控制。
+                * None - 机器人手臂处于未知状态。
+        """
+        return KuavoArmCtrlMode(self._rs_core.arm_control_mode)
+    
+    def manipulation_mpc_ctrl_mode(self) -> KuavoManipulationMpcCtrlMode:
+        """获取 Kuavo 机器人 Manipulation MPC 的当前控制模式。    
+
+        Returns:
+            KuavoManipulationMpcCtrlMode: 当前 Manipulation MPC 控制模式。
+        """
+        return self._rs_core.manipulation_mpc_ctrl_mode
+    
+    def manipulation_mpc_control_flow(self) -> KuavoManipulationMpcControlFlow:
+        """获取 Kuavo 机器人 Manipulation MPC 的当前控制流。
+
+        Returns:
+            KuavoManipulationMpcControlFlow: 当前 Manipulation MPC 控制流。
+        """
+        return self._rs_core.manipulation_mpc_control_flow
+    
+    def manipulation_mpc_frame(self) -> KuavoManipulationMpcFrame:
+        """获取机器人操作MPC的当前帧。
+
+        Returns:
+            KuavoManipulationMpcFrame: 末端执行器 Manipulation MPC 坐标系
+        """
+        return self._rs_core.manipulation_mpc_frame
+    
+    def head_joint_state(self) -> KuavoJointData:
+        """获取机器人头部关节的当前状态。
+
+        获取机器人头部关节的当前状态数据，包括位置、速度、扭矩和加速度值。
+
+        Returns:
+            KuavoJointData: 包含头部关节状态的数据结构:
+                * position (list[float]): 关节位置，单位为弧度，长度=head_dof(2)
+                * velocity (list[float]): 关节速度，单位为rad/s，长度=head_dof(2)
+                * torque (list[float]): 关节扭矩，单位为Nm，长度=head_dof(2)
+                * acceleration (list[float]): 关节加速度，单位为rad/s^2，长度=head_dof(2)
+                
+            关节顺序为 [偏航角, 俯仰角]。
+        """
+        # 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]:
+        """获取机器人末端执行器的当前状态。
+
+        Returns:
+            Tuple[EndEffectorState, EndEffectorState]: 包含左右末端执行器状态的元组。
+                每个EndEffectorState包含:
+                    - position: (float, float, float) ，XYZ位置，单位为米
+                    - orientation: (float, float, float, float) ，四元数方向
+                    - state: EndEffectorState.GraspingState ，当前抓取状态 (UNKNOWN, OPEN, CLOSED)
+        """
+        return copy.deepcopy(self._rs_core.eef_state)
+
+    def gait_name(self)->str:
+        """获取机器人的当前步态名称。
+
+        Returns:
+            str: 当前步态的名称，例如 'trot'、'walk'、'stance'、'custom_gait'。
+        """
+        return self._rs_core.gait_name()
+    
+
+    def is_stance(self) -> bool:
+        """检查机器人当前是否处于站立模式。
+
+        Returns:
+            bool: 如果机器人处于站立模式返回True，否则返回False。
+        """
+        return self._rs_core.is_gait('stance')
+
+    def is_walk(self) -> bool:
+        """检查机器人当前是否处于行走模式。
+
+        Returns:
+            bool: 如果机器人处于行走模式返回True，否则返回False。
+        """
+        return self._rs_core.is_gait('walk')
+
+    def is_step_control(self) -> bool:
+        """检查机器人当前是否处于单步控制模式。
+
+        Returns:
+            bool: 如果机器人处于单步控制模式返回True，否则返回False。
+        """
+        return self._rs_core.is_gait('custom_gait')
+
+    def wait_for_stance(self, timeout:float=5.0)->bool:
+        """等待机器人进入站立模式。
+
+        Args:
+            timeout (float): 等待机器人进入站立状态的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入站立状态返回True，否则返回False。
+        """
+        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:
+        """等待机器人进入踏步状态。
+
+        Args:
+            timeout (float): 等待机器人进入踏步状态的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入踏步状态返回True，否则返回False。
+        """
+        return self.wait_for_walk(timeout=timeout)
+
+    def wait_for_walk(self, timeout:float=5.0)->bool:
+        """等待机器人进入行走模式。
+
+        Args:
+            timeout (float): 等待机器人进入行走状态的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入行走状态返回True，否则返回False。
+        """
+        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:
+        """等待机器人进入单步控制模式。
+
+        Args:
+            timeout (float): 等待机器人进入单步控制模式的最长时间，单位为秒。
+
+        Returns:
+            bool: 如果机器人在指定超时时间内进入单步控制模式返回True，否则返回False。
+        """
+        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,82 @@
+#!/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 KuavoRobotToolsCore
+from typing import Union, Tuple
+
+class KuavoRobotTools:
+    """机器人工具类,提供坐标系转换接口。
+    
+    该类封装了不同机器人坐标系之间的坐标变换查询功能,支持多种返回数据格式。
+    """
+    
+    def __init__(self):
+        self.tools_core = KuavoRobotToolsCore()
+
+    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)
+
+    def get_link_position(self, link_name: str, reference_frame: str = "base_link") -> Union[Tuple[float, float, float], None]:
+        """获取指定机械臂关节链接的位置
+        
+        Args:
+            link_name (str): 关节链接名称，如"zarm_l1_link"
+            reference_frame (str): 参考坐标系，默认为base_link
+            
+        Returns:
+            Tuple[float, float, float] | None: 三维位置坐标(x,y,z)，失败返回None
+        """
+        try:
+            # 获取从参考坐标系到目标链接的变换
+            transform = self.tools_core._get_tf_tree_transform(
+                reference_frame,
+                link_name,
+                return_type="pose_quaternion"
+            )
+            if transform:
+                return transform.position
+            return None
+        except Exception as e:
+            SDKLogger.error(f"获取{link_name}位置失败: {str(e)}")
+            return None