Python 绑定与脚本化控制

本项目通过 pybind11 将底层的 RobotInterface C++ 类封装为原生 Python 扩展模块 robot_py，使开发者能够在不启动 ROS 2 推理节点的前提下，直接用 Python 脚本完成电机初始化、传感器读取与力控指令下发。该机制特别适合硬件调试、出厂标定、单关节测试以及快速算法原型验证等场景。

Sources: pybind_module.cpp, CMakeLists.txt

架构与绑定机制

绑定的核心思路是薄封装、零拷贝、直接映射：C++ 侧的 RobotInterface 已经通过 std::vector<float> 等标准容器与互斥锁实现了线程安全的硬件抽象，pybind11 只需将这些公有方法直接暴露给 Python 运行时，无需额外数据转换层。

在构建层面，CMakeLists.txt 通过 find_package(pybind11 REQUIRED) 引入 pybind11 工具链，再调用 pybind11_add_module(robot_py src/pybind_module.cpp) 生成共享库。该共享库链接到包含全部硬件驱动逻辑的 robot 静态库，确保 Python 模块拥有与 C++ 可执行文件完全一致的功能边界。安装阶段，模块被部署到 lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages，从而在进入 Python 虚拟环境或 ROS 2 工作空间的 Python 路径后，可直接 import robot_py。

Sources: CMakeLists.txt, CMakeLists.txt

下图展示了 Python 绑定在整个软件栈中的位置。与通过 ROS 2 话题间接控制不同，Python 脚本在这里直接持有 RobotInterface 实例，与 C++ 推理节点处于同一抽象层级。

flowchart TB
    subgraph Python_Runtime["Python 运行时"]
        Script["用户脚本 (import robot_py)"]
    end
    subgraph Native_Layer["原生层 (C++)"]
        PyModule["robot_py 模块<br/>(pybind11)"]
        RobotIface["RobotInterface"]
        MotorDrv["MotorDriver"]
        IMUDrv["IMUDriver"]
        Decouple["闭环解耦 (Decouple)"]
    end
    Script -->|"调用 apply_action / get_joint_q 等"| PyModule
    PyModule -->|"直接映射"| RobotIface
    RobotIface -->|"CAN/CANFD"| MotorDrv
    RobotIface -->|"Serial"| IMUDrv
    RobotIface -->|"计算"| Decouple

Sources: pybind_module.cpp, robot_interface.hpp

Python API 详解

robot_py 模块目前仅导出单个类 RobotInterface，其接口设计与 C++ 侧保持命名一致，便于在 C++ 算法与 Python 脚本之间无缝切换。

方法 / 属性	参数类型	返回值	说明
`RobotInterface(config_file)`	`str`	实例	构造函数，加载 YAML 配置并初始化 IMU 与电机驱动映射
`apply_action(action)`	`List[float]`	`None`	下发目标位置/力矩动作，内部自动处理闭环解耦与 MIT 指令格式
`init_motors()`	`None`	`None`	使能全部电机
`deinit_motors()`	`None`	`None`	关闭电机并释放使能
`reset_joints(joint_default_angle)`	`List[float]`	`None`	以分阶段降低刚度的方式将关节复位至默认角度
`set_zeros()`	`None`	`None`	将当前电机位置标定为零位
`clear_errors()`	`None`	`None`	清除电机报警状态
`refresh_joints()`	`None`	`None`	强制刷新并读取一次关节状态（含闭环解耦计算）
`get_joint_q()`	`None`	`List[float]`	获取关节位置（弧度），电机未初始化会抛异常
`get_joint_vel()`	`None`	`List[float]`	获取关节速度
`get_joint_tau()`	`None`	`List[float]`	获取关节电流/力矩
`get_quat()`	`None`	`List[float]`	获取 IMU 四元数 `[w, x, y, z]`，已乘外参旋转矩阵
`get_ang_vel()`	`None`	`List[float]`	获取角速度，已完成坐标系转换
`is_init`	只读属性	`bool`	电机是否已完成初始化

需要注意，get_joint_q、get_joint_vel、get_joint_tau 与 get_quat、get_ang_vel 内部均持有互斥锁，在电机或 IMU 未就绪时会抛出 std::runtime_error，pybind11 会自动将其转换为 Python 的 RuntimeError。这一行为与 C++ 侧完全一致，脚本层应通过 try/except 捕获或先检查 is_init 属性。

Sources: robot_interface.hpp, pybind_module.cpp

脚本化控制示例

以下示例展示了如何使用 robot_py 完成一次典型的硬件调试流程：加载配置、初始化电机、读取传感器、下发零位动作。该脚本无需 ROS 2 环境即可运行，前提是已将 robot_py 模块安装到当前 Python 解释器的搜索路径中。

import robot_py
import time

# 1. 加载硬件配置
robot = robot_py.RobotInterface("config/robot.yaml")

# 2. 使能电机
robot.init_motors()
print("Motor init state:", robot.is_init)

# 3. 读取当前关节状态
q = robot.get_joint_q()
print("Current joint positions:", q)

# 4. 读取 IMU
quat = robot.get_quat()
omega = robot.get_ang_vel()
print("IMU quat:", quat)
print("IMU ang_vel:", omega)

# 5. 下发零位保持指令（23 自由度示例）
action = [0.0] * 23
robot.apply_action(action)

time.sleep(1.0)

# 6. 关闭电机
robot.deinit_motors()

若需要进行关节标定，可使用 set_zeros() 与 reset_joints() 的组合。reset_joints 内部采用分阶段刚度策略：先以较低的 kp 运行 1 秒，再切换至额定 kp/kd，避免机器人因初始位姿偏差产生剧烈抖动。调用前请确保已通过 apply_action 或手动方式将机器人调整至接近目标姿态的安全状态。

Sources: robot_interface.cpp, pybind_module.cpp

与 ROS 2 推理节点的对比

同一套 RobotInterface 能力在系统中有两条使用路径：一条通过 robot_py 供 Python 脚本直接调用，另一条被 inference_node 内部持有，经由 ROS 2 话题与服务与上层交互。两者并非互斥，而是面向不同开发阶段。

维度	Python 绑定 (`robot_py`)	ROS 2 推理节点 (`inference_node`)
启动依赖	仅需硬件驱动与 Python 环境	需完整 ROS 2 生态与 Launch 文件
控制延迟	直接函数调用，最低开销	经 ROS 2 回调队列，存在话题延迟
多策略切换	需脚本自行实现加载与切换逻辑	原生支持热切换，参考多策略运行时与热切换机制
观测值组装	脚本自行读取原始传感器数据	由 `ObsManager` 完成归一化与帧堆叠，参考观测值组装、归一化与帧堆叠
安全监控	依赖脚本自身实现	节点内置故障检测与保护逻辑，参考安全监控、限位与故障处理
适用场景	单关节调试、标定、教学演示	实时推理部署、多模块协同

Sources: inference_node.hpp, launch/inference.launch.py

构建与路径配置

在 colcon 工作空间中编译本包时，robot_py 会随 roboto_inference 一起构建。由于 CMake 使用了 pybind11_add_module，编译产物为 robot_py*.so。安装路径遵循 ROS 2 的 Python 包规范，位于 install/lib/python3.x/site-packages/ 下。若希望在任意终端中导入该模块，需确保以下环境变量已正确配置：

# 假设工作空间为 ~/roboto_ws
source ~/roboto_ws/install/setup.bash
python3 -c "import robot_py; print(robot_py.__file__)"

若出现 ModuleNotFoundError，请检查 PYTHONPATH 是否包含 install/lib/python3.x/site-packages。在某些嵌入式平台（aarch64）上，还需确认 LD_LIBRARY_PATH 已指向 libonnxruntime.so 与 libcnpy.so 所在目录，尽管 robot_py 本身不依赖 ONNX Runtime，但其链接的 robot 静态库可能通过间接符号表引入运行时路径要求。

Sources: CMakeLists.txt

故障排查

现象	可能原因	处理建议
`ModuleNotFoundError: No module named 'robot_py'`	未 source 工作空间或安装路径未加入 `PYTHONPATH`	执行 `source install/setup.bash` 并核对 Python 版本
`RuntimeError: Motors not initialized`	调用 `get_joint_q` 等前未执行 `init_motors()`	在读取传感器前显式调用 `init_motors()`
`RuntimeError: Motor id X offline`	对应电机通信中断或供电异常	检查 CAN 接线与电源，参考电机驱动与 CAN/CANFD 通信
关节读数方向相反	`motor_sign` 配置错误	修改 `config/robot.yaml` 中的 `motor_sign` 并重新编译安装
`reset_joints` 后机器人抖动剧烈	默认角度与实际姿态偏差过大	先手动扶稳或降低 `kp`，再逐步逼近目标角度

Sources: robot_interface.cpp, config/robot.yaml