本项目提供了一套基于 pybind11 封装的 Python SDK，允许你在不编写 C++ 代码的前提下，直接通过 Python 脚本控制电机、读取 IMU 姿态，甚至驱动整台机器人完成复杂动作。SDK 包含三个核心模块：motors_py（电机驱动）、imu_py（IMU 驱动）与 robot_py（整机硬件抽象）。本页面向初学者，带你从环境准备到完整示例，逐步掌握各模块的用法。

Sources: README_CN.md

前置准备：编译与环境激活

Python SDK 并非独立安装的 pip 包，而是 C++ 源码通过 pybind11 编译后生成的共享库。因此，在导入任何模块之前，你必须先完成工作空间的编译，并正确 source 环境。

执行以下命令完成编译：

colcon build --symlink-install
source /opt/ros/humble/setup.bash
source install/setup.bash

编译成功后，motors_py、imu_py 和 robot_py 三个模块会被安装到 Python 路径中，此时即可在脚本中直接 import。若遇到 ModuleNotFoundError，请首先检查是否已执行上述 source 命令，并确认 colcon build 没有报错。

Sources: README_CN.md

SDK 架构概览

三个模块呈分层结构：motors_py 与 imu_py 直接面向底层硬件总线，分别通过 CAN/CANFD 和串口与设备通信；robot_py 则在内部聚合了电机管理与 IMU 读取逻辑，对外提供统一的配置文件驱动接口，适合需要同时操控多台电机并读取机体姿态的场景。

graph TD
    subgraph Python_SDK["Python SDK"]
        direction TB
        A[用户脚本] --> B[robot_py<br/>RobotInterface]
        A --> C[motors_py<br/>MotorDriver]
        A --> D[imu_py<br/>IMUDriver]
    end
    
    B --> E[C++ RobotInterface<br/>多电机 + IMU 管理]
    C --> F[C++ MotorDriver<br/>单电机控制]
    D --> G[C++ IMUDriver<br/>姿态传感器]
    
    E --> H[CAN0/1/2/3 总线]
    E --> I[串口 /dev/ttyUSB0]
    F --> H
    G --> I

对于仅需测试单台电机或读取 IMU 数据的快速验证场景，直接使用 motors_py 或 imu_py 最为轻量；而对于控制整台机器人（如 23 自由度的人形机器人），推荐使用 robot_py，因为它自动处理电机编组、关节映射、线程池并行通信以及闭链运动学解耦等复杂逻辑。

Sources: src/inference/include/robot_interface.hpp

模块一：电机驱动 motors_py

motors_py 封装了底层电机驱动类，目前支持达妙（DM）系列、EVO 系列等主流电机。每个电机对象通过 MotorDriver.create_motor() 静态工厂方法创建，随后可在 MIT 模式（阻抗控制）、位置模式或速度模式下运行。

核心 API

create_motor 参数详解：

Sources: src/motors/src/pybind_module.cpp

完整示例：单电机 MIT 控制

以下示例演示了如何创建一台 ID 为 1 的电机，将其切换到 MIT 模式，发送一个目标位置指令，并读取反馈状态。该代码完整摘自 scripts/motors_py_example.py，可直接运行验证。

#!/usr/bin/env python3
import motors_py
import time

def example_can_motor():
    print("=== CAN电机示例 ===")
    motors = []
    try:
        motor = motors_py.MotorDriver.create_motor(
            motor_id=1,
            interface_type="can",
            interface="can0",
            motor_type="DM",
            motor_model=0,
            master_id_offset=16,
        )
        motors.append(motor)
        print("电机创建成功！")
    except Exception as e:
        print(f"创建电机失败: {e}")
        return

    try:
        print("使能电机...")
        for motor in motors:
            motor.init_motor()

        print("\n=== MIT模式控制示例 ===")
        motors[0].set_motor_control_mode(motors_py.MotorControlMode.MIT)

        target_pos = -0.5
        target_vel = 0.0
        kp = 5.0
        kd = 1.0
        torque = 0.0

        motors[0].motor_mit_cmd(target_pos, target_vel, kp, kd, torque)

        # 读取电机状态
        pos = motors[0].get_motor_pos()
        vel = motors[0].get_motor_spd()
        current = motors[0].get_motor_current()
        temp = motors[0].get_motor_temperature()
        error_id = motors[0].get_error_id()

        print(f"位置: {pos:.4f} rad, 速度: {vel:.4f} rad/s, "
              f"电流: {current:.4f} A, 温度: {temp:.2f}°C, 错误码: {error_id}")
        time.sleep(1)
    except Exception as e:
        print(f"电机控制过程中出错: {e}")
    finally:
        for motor in motors:
            print("失能电机...")
            motor.deinit_motor()

if __name__ == "__main__":
    example_can_motor()

关键流程如下：

flowchart LR
    A[create_motor<br/>创建实例] --> B[init_motor<br/>使能电机]
    B --> C[set_motor_control_mode<br/>设为MIT模式]
    C --> D[motor_mit_cmd<br/>发送控制指令]
    D --> E[get_motor_pos<br/>读取反馈]
    E --> D
    D --> F[deinit_motor<br/>失能电机]

Sources: scripts/motors_py_example.py

模块二：IMU 驱动 imu_py

imu_py 用于读取 IMU 的姿态四元数、角速度、线加速度及温度数据。目前默认支持 HiPNUC 系列 IMU，通过串口通信。与 motors_py 类似，它同样采用工厂方法 create_imu() 创建实例。

核心 API

Sources: src/imu/src/pybind_module.cpp

完整示例：串口 IMU 数据读取

以下示例从 /dev/ttyUSB0 以 921600 波特率连接 IMU，循环 1000 次读取并打印姿态数据。

#!/usr/bin/env python3
import imu_py
import time

def example_serial_imu():
    print("=== 串口IMU示例 ===")
    try:
        imu = imu_py.IMUDriver.create_imu(
            imu_id=8,
            interface_type="serial",
            interface="/dev/ttyUSB0",
            imu_type="HIPNUC",
            baudrate=921600
        )
    except Exception as e:
        print(f"创建IMU失败: {e}")
        return

    print(f"IMU ID: {imu.get_imu_id()}")

    for i in range(1000):
        quat = imu.get_quat()
        print(f"四元数: w={quat[0]:.4f}, x={quat[1]:.4f}, y={quat[2]:.4f}, z={quat[3]:.4f}")

        ang_vel = imu.get_ang_vel()
        print(f"角速度: x={ang_vel[0]:.4f}, y={ang_vel[1]:.4f}, z={ang_vel[2]:.4f} rad/s")

        lin_acc = imu.get_lin_acc()
        print(f"线加速度: x={lin_acc[0]:.4f}, y={lin_acc[1]:.4f}, z={lin_acc[2]:.4f} m/s^2")

        temp = imu.get_temperature()
        print(f"温度: {temp:.2f}°C")

        print("-" * 50)
        time.sleep(0.01)

if __name__ == "__main__":
    example_serial_imu()

Sources: scripts/imu_py_example.py

模块三：整机控制 robot_py

当你需要同时管理 20 多台电机并实时读取 IMU 时，逐个调用 motors_py 创建实例会变得繁琐且容易出错。robot_py.RobotInterface 通过读取 YAML 配置文件，在内部自动完成电机编组、IMU 初始化、线程池创建和关节映射，对外仅暴露简洁的高层接口。

核心 API

Sources: src/inference/src/pybind_module.cpp

配置文件说明

robot_py 依赖 YAML 配置文件来描述机器人的硬件拓扑。配置文件通常包含 motors（电机列表、CAN 接口分配、型号、零点偏移）和 robot（控制参数 kp/kd、电机方向 motor_sign、闭链电机 close_chain_motor_id、URDF 映射 urdf2motor、外参旋转矩阵 extrinsic_R）两大部分。以下是一个简化示意：

motors:
    motor_id: [1, 2, 3, ...]
    motor_interface_type: "can"
    motor_interface: ["can0", "can1", "can2", "can3"]
    motor_num: [6, 7, 5, 5]
    motor_type: "DM"
    motor_model: [1, 1, 0, ...]
    master_id_offset: 16
    motor_zero_offset: [0.0, 0.0, ...]

robot:
    kp: [100.0, 100.0, ...]
    kd: [3.3, 3.3, ...]
    motor_sign: [1, 1, -1, ...]
    close_chain_motor_id: [5, 6, 11, 12]
    urdf2motor: [0, 1, 2, ...]
    extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]

实际使用时，你只需修改 motor_zero_offset、motor_model 等与实际硬件对应的字段，无需改动代码即可适配不同构型。

Sources: scripts/config/motion_player.yaml

使用示例：初始化并发送动作

import robot_py

# 1. 根据配置文件创建整机接口
robot = robot_py.RobotInterface("config/motion_player.yaml")

# 2. 批量初始化所有电机
robot.init_motors()

# 3. 应用动作（23 个关节的目标位置）
robot.apply_action([0.0] * 23)

# 4. 读取反馈
joint_pos = robot.get_joint_q()
quat = robot.get_quat()

Sources: README_CN.md

实战：电机零点标定

零点标定是机器人首次使用前必须完成的步骤。仓库提供了 scripts/set_zero.py，它基于 motors_py 实现了一个交互式标定流程：脚本会依次创建电机、将其切换到 MIT 阻尼模式（便于手动转动），然后等待用户将电机摆到机械零位并按回车确认。

该脚本的核心逻辑分为三步：首先通过 create_motor() 批量创建电机对象，随后在 calibrate_motor() 中调用 init_motor() 和 motor_mit_cmd(0.0, 0.0, 0.0, 1.0, 0.0) 进入低刚度阻尼状态，最后用户确认后调用 set_motor_zero() 写入零点。这种单电机逐个标定的方式特别适合首次装机或部分电机更换后的场景。

# 核心逻辑示意（摘自 set_zero.py）
motor.init_motor()
motor.set_motor_control_mode(motors_py.MotorControlMode.MIT)

while True:
    motor.motor_mit_cmd(0.0, 0.0, 0.0, 1.0, 0.0)
    pos = motor.get_motor_pos()
    # ... 按 Enter 确认，按空格跳过
    motor.set_motor_zero()
    break

motor.deinit_motor()

运行前请检查 scripts/config/set_zero.yaml 中的 motor_id、motor_interface 和 motor_num 是否与你的实际连线一致。

Sources: scripts/set_zero.py

实战：动作序列播放

scripts/motion_player.py 展示了如何使用 robot_py 播放一段预录制的动作序列。它首先通过 numpy 加载 .npz 动作文件，然后使用 MotionLoader 解析关节位置与速度，最后通过 RobotInterface 以 200Hz 的控制频率将动作逐帧下发到电机。

该示例的要点在于 MotionPlayer 类的 step_once() 方法：它根据播放速度和动作文件的原始 FPS 计算帧间隔，在合适的时刻调用 update_frame() 获取新的目标位置，并在每一控制周期调用 robot.apply_action(self.positions) 将指令发送给底层。通过这种方式，robot_py 屏蔽了多电机并行通信和关节映射的细节，让你可以专注于上层动作逻辑。

class MotionPlayer:
    def __init__(self, motion_file, config_file, speed=1.0):
        self.motion_loader = MotionLoader(motion_file, self.logger)
        self.robot = robot_py.RobotInterface(config_file)
        self.robot.init_motors()
        self.robot.reset_joints(self.motion_loader.joint_default_angle)

    def step_once(self):
        if self.i % self.step == 0:
            frame_idx = self.i // self.step
            self.update_frame(frame_idx)
        self.robot.apply_action(self.positions)
        self.i += 1

Sources: scripts/motion_player.py

常见问题与排查

下一步

掌握了 Python SDK 的基础用法后，你可以根据实际需求深入以下方向：

• 若需理解 robot_py 内部的线程模型与多电机并行通信机制，请阅读 RobotInterface 硬件抽象层。
• 若计划接入自定义强化学习策略并替换默认推理逻辑，请参考 接入自定义强化学习策略。
• 若希望了解电机底层通信协议与 MIT 控制模式的数学原理，请查看 电机驱动模块与 MIT 控制 和 通信协议层：CAN/CANFD 与串口。