增加ESP32S3实现代码，在本地端通过wifi与其进行通信，通过CAN与RS00关节电机进行通信

README.md

@@ -89,6 +89,16 @@ ros2 launch guguji_ros2 gazebo.launch.py gui:=false pause:=true
 
 分开管理，后续会更容易调试。
 
+## 真实机器人部署工程
+
+仓库根目录下新增了 `guguji_real_robot/`，用于把训练后的策略部署到真实双足机器人：
+
+- `host/`：本机加载训练模型，通过 WiFi UDP 输出 8 个关节目标。
+- `firmware/esp32s3_espidf/`：ESP32S3 固件，接收 UDP 命令并通过 TWAI/CAN 控制 RS00。
+- `docs/`：接线、刷机、部署、RS00 MIT 协议和安全调试说明。
+
+建议从 `guguji_real_robot/docs/real_robot_deployment_guide.md` 开始阅读。
+
 ## 控制示例
 
 例如把左膝关节目标角度设置为 `0.8 rad`：

guguji_real_robot/README.md

@@ -0,0 +1,23 @@
+# guguji_real_robot
+
+这个目录用于把 `guguji_rl` 训练好的双足策略部署到真实机器人。
+
+当前实现采用“本机推理 + ESP32S3 执行”的低成本方案：
+
+1. 本机加载 stable-baselines3 PPO 模型。
+2. 本机按训练配置重建观测和动作映射，输出 8 个关节目标角。
+3. 本机通过 WiFi UDP 把目标角、速度、Kp、Kd、前馈力矩发给 ESP32S3。
+4. ESP32S3 用 TWAI/CAN 按 RS00 MIT 协议控制关节电机。
+5. ESP32S3 回传电机反馈、QMI8658A 姿态估计和 QMC6309 地磁数据。
+6. GPIO42 蜂鸣器和 GPIO45 WS2812 负责无屏状态提示。
+
+目录说明：
+
+- `host/`：本机运行模型和 UDP 桥接的 Python 代码。
+- `firmware/esp32s3_espidf/`：ESP-IDF 固件工程。
+- `docs/`：接线、刷机、部署、标定和安全测试说明。
+
+建议先读：
+
+- [真实机器人部署教程](docs/real_robot_deployment_guide.md)
+- [RS00 MIT 协议摘要](docs/rs00_mit_protocol_notes.md)

guguji_real_robot/docs/real_robot_deployment_guide.md

@@ -0,0 +1,224 @@
+# 真实机器人部署教程
+
+## 1. 技术选择
+
+本项目第一版选择 ESP-IDF，而不是 Arduino。
+
+原因是 ESP32S3 需要同时处理 WiFi UDP、TWAI/CAN、I2C 传感器、命令超时保护和多任务控制循环。ESP-IDF 对 FreeRTOS、WiFi、TWAI 和 I2C 的控制更直接，后续调试真实机器人会更稳。
+
+当前闭环是：
+
+```text
+本机 PPO 模型
+  -> UDP 关节命令
+  -> ESP32S3
+  -> TWAI/CAN
+  -> RS00 关节电机
+  -> CAN 反馈 + IMU/地磁遥测
+  -> UDP 回本机
+```
+
+## 2. 硬件连接
+
+你的 ESP32S3 控制板已经集成 TJA1050T CAN 总线收发器，因此控制板可以直接通过 CANH/CANL 与 RS00 关节电机总线通信。
+
+当前默认引脚：
+
+- ESP32S3 `GPIO5` -> TJA1050T TXD
+- ESP32S3 `GPIO4` -> TJA1050T RXD
+- TJA1050T CANH/CANL -> RS00 CANH/CANL 总线
+- 总线两端各 120Ω 终端电阻
+- ESP32S3 GND、RS00 电源 GND 共地
+- RS00 电机按手册供电，额定 48V，允许 24V-60V
+
+无屏状态提示硬件：
+
+- `GPIO42` -> 无源蜂鸣器
+- `GPIO45` -> WS2812 三色 LED
+
+I2C 默认连接：
+
+- `GPIO8` -> SDA
+- `GPIO9` -> SCL
+- QMI8658A 默认地址 `0x6B`
+- QMC6309 默认地址 `0x7C`
+
+如果你的板子引脚不同，修改：
+
+```text
+guguji_real_robot/firmware/esp32s3_espidf/main/config.h
+```
+
+## 3. 电机准备
+
+RS00 手册说明电机默认 CAN 波特率为 1Mbps。本固件也按 1Mbps 初始化 TWAI。
+
+建议每个电机设置唯一 CAN ID：
+
+```text
+left_hip_pitch_joint      -> 1
+left_knee_pitch_joint     -> 2
+left_ankle_pitch_joint    -> 3
+left_ankle_joint          -> 4
+right_hip_pitch_joint     -> 5
+right_knee_pitch_joint    -> 6
+right_ankle_pitch_joint   -> 7
+right_ankle_joint         -> 8
+```
+
+对应配置在：
+
+```text
+guguji_real_robot/firmware/esp32s3_espidf/main/robot_config.h
+```
+
+如果你的电机还在私有协议或 CANopen 协议，需要先用官方上位机或 CAN 工具切到 MIT 协议，并重新上电生效。
+
+## 4. 编译和刷写 ESP32S3
+
+安装 ESP-IDF 后执行：
+
+```bash
+cd /home/corvin/Project/guguji_simulation/guguji_real_robot/firmware/esp32s3_espidf
+idf.py set-target esp32s3
+idf.py build
+idf.py -p /dev/ttyUSB0 flash monitor
+```
+
+固件默认会启动 WiFi 热点：
+
+```text
+SSID: guguji-robot
+Password: guguji1234
+ESP32S3 IP: 192.168.4.1
+```
+
+电脑连接这个热点后，主机端配置默认即可使用。
+
+## 5. 无屏状态提示
+
+固件会用蜂鸣器和 WS2812 提示当前状态：
+
+```text
+蓝灯慢闪             等待本机主机连接
+青色慢闪             已连接但电机失能，或处于安全保持
+绿灯常亮             已 ARM，正在向电机下发控制
+黄灯双闪 + 双短鸣     主机命令超时，电机已停止
+红灯快闪 + 低频鸣叫   急停或 RS00 电机故障
+紫灯慢闪 + 高低短鸣   传感器离线或电机反馈超时等警告
+```
+
+这些提示只表示固件判断到的状态，真正落地测试时仍然要保留物理急停和电源限流。
+
+## 6. 本机部署模型
+
+先安装依赖：
+
+```bash
+cd /home/corvin/Project/guguji_simulation
+python3 -m venv .venv-real
+source .venv-real/bin/activate
+pip install -r guguji_rl/requirements.txt
+pip install -r guguji_real_robot/host/requirements.txt
+```
+
+不使能电机，仅检查遥测和模型输出：
+
+```bash
+python guguji_real_robot/host/scripts/run_real_policy.py \
+  --model guguji_rl/outputs/<你的训练目录>/final_model.zip \
+  --deterministic
+```
+
+主机端会周期性打印 ESP32S3 遥测，例如：
+
+```text
+[ESP32S3] state=DISARMED seq=12 uptime=8.4s cmd_age=16ms faults=ok rpy=(0.3,-1.1,12.6)deg temp_max=32.5C joint_vel_mean=0.018rad/s
+```
+
+如果出现问题，会把故障拆开显示：
+
+```text
+faults=feedback_timeout=left_knee_pitch_joint; imu_offline
+```
+
+确认机器人悬空、急停开关可用、电源限流合理后，再加 `--arm`：
+
+```bash
+python guguji_real_robot/host/scripts/run_real_policy.py \
+  --model guguji_rl/outputs/<你的训练目录>/final_model.zip \
+  --deterministic \
+  --clear-faults \
+  --arm
+```
+
+可用 `--log-interval 0.5` 临时提高日志刷新率。默认值在：
+
+```text
+guguji_real_robot/host/configs/guguji_real_robot.yaml
+```
+
+## 7. 方向和零点标定
+
+第一次不要让机器人落地，务必悬空测试。
+
+标定步骤：
+
+1. 使用官方工具或本固件的零偏配置，让每个关节机械零位和仿真名义站姿对应。
+2. 单独给每个关节一个很小的正向目标角，观察真实转动方向。
+3. 如果方向相反，在 `robot_config.h` 中把该关节 `direction` 从 `1.0f` 改成 `-1.0f`。
+4. 如果零位有偏差，修改 `zero_offset_rad`。
+5. 每次修改后重新编译刷写。
+
+映射关系是：
+
+```text
+motor_angle = joint_angle * direction + zero_offset_rad
+```
+
+## 8. 安全参数
+
+主机侧部署安全参数在：
+
+```text
+guguji_real_robot/host/configs/guguji_real_robot.yaml
+```
+
+重点参数：
+
+- `max_joint_step_rad`：每个控制周期允许目标角变化的最大值。
+- `max_roll_rad` / `max_pitch_rad`：超过后主机请求急停。
+- `kp` / `kd`：RS00 MIT 运控模式的关节增益。
+
+固件侧安全参数在 `config.h`：
+
+- `GUGUJI_COMMAND_TIMEOUT_MS`：超过这个时间没有收到主机命令就停止电机。
+- `GUGUJI_CONTROL_PERIOD_MS`：CAN 控制周期，默认 20ms，与当前训练 `control_dt=0.05s` 相比更快，主机会按训练周期发新目标，固件保持最近目标。
+- `GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS`：超过这个时间没有收到某个 RS00 的反馈，就在遥测中标记该关节反馈超时。
+
+## 9. 当前限制
+
+现有训练观测包含：
+
+- 关节位置
+- 关节速度
+- 上一时刻动作
+- base height
+- roll / pitch
+- forward velocity
+- target forward velocity
+
+真实机器人当前只有关节反馈和 IMU，没有外部定位或测高，所以 `base height` 和 `forward velocity` 先由主机配置里的常量补齐。这能用于初步看策略摆腿效果，但要让行走策略更接近仿真，后续建议接入以下任一方案：
+
+- 动捕或视觉里程计，用于真实 forward velocity。
+- 足底接触或测距传感器，用于 base height。
+- 重新训练一个不依赖外部速度和高度的部署策略。
+
+## 10. 推荐调试顺序
+
+1. 只刷固件，查看串口日志确认 WiFi、QMI8658A、QMC6309、蜂鸣器、WS2812 初始化。
+2. 不接电机，只运行主机脚本，确认 UDP 遥测正常。
+3. 只接 1 个 RS00，确认 CAN ID、使能、停止、反馈解析。
+4. 悬空接 8 个电机，逐个校准方向和零位。
+5. 用低 Kp/Kd、低 `max_joint_step_rad` 运行 `--arm`。
+6. 确认不会突然打限位后，再逐步提高增益和落地测试。

guguji_real_robot/docs/rs00_mit_protocol_notes.md

@@ -0,0 +1,78 @@
+# RS00 MIT 协议摘要
+
+依据附件 `RS00使用说明书260428.pdf` 第 6 章整理。本文只记录本项目固件实际用到的部分。
+
+## 1. 基本信息
+
+- CAN 2.0 标准帧。
+- 默认波特率 1Mbps。
+- MIT 模式支持一帧下发位置、速度、Kp、Kd、前馈力矩。
+- RS00 额定负载 5N.m，峰值负载 14N.m。
+- 电机工作电压范围 24V-60V，额定 48V。
+
+## 2. 使能、停止和清错
+
+对目标电机 CAN ID 发送标准帧，数据区如下：
+
+```text
+使能: FF FF FF FF FF FF FF FC
+停止: FF FF FF FF FF FF FF FD
+清错: FF FF FF FF FF FF FF FB
+```
+
+本固件在收到主机 `ARM` 命令后会先发 MIT 模式设置，再发使能；收到 `DISARM`、`ESTOP` 或命令超时后发送停止帧。
+
+## 3. MIT 动态控制帧
+
+发送给目标电机 CAN ID，8 字节数据打包：
+
+```text
+Byte0~1: 目标角度 p，uint16，对应 -12.57rad ~ 12.57rad
+Byte2 + Byte3[7:4]: 目标速度 v，uint12，对应 -33rad/s ~ 33rad/s
+Byte3[3:0] + Byte4: Kp，uint12，对应 0 ~ 500
+Byte5 + Byte6[7:4]: Kd，uint12，对应 0 ~ 5
+Byte6[3:0] + Byte7: 前馈力矩 t，uint12，对应 -14N.m ~ 14N.m
+```
+
+本固件实现位置在：
+
+```text
+guguji_real_robot/firmware/esp32s3_espidf/main/rs00_mit.c
+```
+
+核心公式：
+
+```text
+uint = (value - min) * (2^bits - 1) / (max - min)
+```
+
+## 4. 反馈帧
+
+电机反馈帧数据区：
+
+```text
+Byte0: 电机 CAN ID
+Byte1~2: 当前角度，uint16，对应 -12.57rad ~ 12.57rad
+Byte3 + Byte4[7:4]: 当前速度，uint12，对应 -33rad/s ~ 33rad/s
+Byte4[3:0] + Byte5: 当前力矩，uint12，对应 -14N.m ~ 14N.m
+Byte6[7:6]: 模式状态，0 Reset，1 Cali，2 Motor
+Byte6[5]: 故障标志
+Byte6[4]: 预警标志
+Byte6[3:0] + Byte7: 温度，单位 0.1 摄氏度
+```
+
+固件会把反馈转换为仿真关节角，再通过 UDP 遥测回传给主机。
+
+## 5. 与本项目相关的注意事项
+
+- 手册明确说明关节运行时不要直接切换控制方式，切换前应先停止运行。
+- 命令超时保护应打开或由上层实现。本项目固件侧默认 200ms 未收到 UDP 命令即停止全部电机。
+- 真机测试前先悬空，低增益、低目标角变化率验证方向。
+- 位置范围和仿真 URDF 临时关节范围不同步时，应优先保护真实机械限位。
+
+## 6. 传感器资料来源
+
+传感器寄存器初始化参考了公开数据手册：
+
+- QMI8658A Datasheet: https://files.waveshare.com/upload/5/5f/QMI8658A_Datasheet_Rev_A.pdf
+- QMC6309 Datasheet: https://uploadcdn.oneyac.com/attachments/files/brand_pdf/qst/26/AF/QMC6309.pdf

guguji_real_robot/firmware/esp32s3_espidf/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(guguji_esp32s3_bridge)

guguji_real_robot/firmware/esp32s3_espidf/README.md

@@ -0,0 +1,38 @@
+# ESP32S3 固件
+
+这是 guguji 真实机器人桥接固件，基于 ESP-IDF。
+
+默认功能：
+
+- WiFi AP：`guguji-robot` / `guguji1234`
+- UDP 命令端口：`7777`
+- UDP 遥测回传：发给最近一次命令来源地址
+- TWAI/CAN：1Mbps，控制 RS00 MIT 协议电机
+- I2C：读取 QMI8658A 和 QMC6309
+- GPIO42：无源蜂鸣器状态提示
+- GPIO45：WS2812 RGB LED 状态提示
+- 20ms 控制周期，200ms 命令超时停止电机
+
+编译：
+
+```bash
+idf.py set-target esp32s3
+idf.py build
+```
+
+刷写：
+
+```bash
+idf.py -p /dev/ttyUSB0 flash monitor
+```
+
+请先修改 `main/config.h` 中的引脚，再修改 `main/robot_config.h` 中的 CAN ID、方向和零偏。
+
+状态提示：
+
+- 蓝灯慢闪：等待主机
+- 青色慢闪：失能/安全保持
+- 绿灯常亮：运行中
+- 黄灯双闪：命令超时
+- 红灯快闪：急停或电机故障
+- 紫灯慢闪：传感器离线或反馈超时警告

guguji_real_robot/firmware/esp32s3_espidf/main/CMakeLists.txt

@@ -0,0 +1,9 @@
+idf_component_register(
+    SRCS
+        "app_main.c"
+        "guguji_protocol.c"
+        "rs00_mit.c"
+        "sensors.c"
+        "status_outputs.c"
+    INCLUDE_DIRS "."
+)

guguji_real_robot/firmware/esp32s3_espidf/main/app_main.c

@@ -0,0 +1,515 @@
+#include <errno.h>
+#include <inttypes.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "config.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "esp_netif.h"
+#include "esp_timer.h"
+#include "esp_wifi.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/event_groups.h"
+#include "freertos/semphr.h"
+#include "freertos/task.h"
+#include "lwip/inet.h"
+#include "lwip/sockets.h"
+#include "nvs_flash.h"
+
+#include "guguji_protocol.h"
+#include "robot_config.h"
+#include "rs00_mit.h"
+#include "sensors.h"
+#include "status_outputs.h"
+
+static const char *TAG = "guguji_bridge";
+#if !GUGUJI_WIFI_USE_AP
+static const int WIFI_CONNECTED_BIT = BIT0;
+#endif
+
+static SemaphoreHandle_t g_command_mutex;
+static SemaphoreHandle_t g_feedback_mutex;
+static SemaphoreHandle_t g_sensor_mutex;
+static SemaphoreHandle_t g_peer_mutex;
+#if !GUGUJI_WIFI_USE_AP
+static EventGroupHandle_t g_wifi_event_group;
+#endif
+
+static guguji_command_packet_t g_latest_command;
+static bool g_have_command = false;
+static uint32_t g_last_command_ms = 0;
+static uint32_t g_latest_command_sequence = 0;
+
+static rs00_feedback_t g_feedback[GUGUJI_JOINT_COUNT];
+static guguji_sensor_state_t g_sensor_state;
+static guguji_robot_state_t g_robot_state = GUGUJI_STATE_DISARMED;
+static volatile uint32_t g_latest_fault_mask = GUGUJI_FAULT_IMU_OFFLINE | GUGUJI_FAULT_MAG_OFFLINE;
+static struct sockaddr_storage g_last_peer_addr;
+static socklen_t g_last_peer_len = 0;
+static bool g_have_peer = false;
+
+static uint32_t now_ms(void)
+{
+    return (uint32_t)(esp_timer_get_time() / 1000ULL);
+}
+
+static float clampf_local(float value, float min_value, float max_value)
+{
+    if (value < min_value) {
+        return min_value;
+    }
+    if (value > max_value) {
+        return max_value;
+    }
+    return value;
+}
+
+static int find_joint_by_motor_id(uint8_t motor_id)
+{
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        if (GUGUJI_JOINTS[i].can_id == motor_id) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+static float joint_to_motor_angle(int index, float joint_angle)
+{
+    const guguji_joint_config_t *joint = &GUGUJI_JOINTS[index];
+    return joint_angle * joint->direction + joint->zero_offset_rad;
+}
+
+static float motor_to_joint_angle(int index, float motor_angle)
+{
+    const guguji_joint_config_t *joint = &GUGUJI_JOINTS[index];
+    return (motor_angle - joint->zero_offset_rad) * joint->direction;
+}
+
+static void stop_all_motors(void)
+{
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        rs00_mit_send_stop(GUGUJI_JOINTS[i].can_id);
+    }
+}
+
+static void clear_all_motor_faults(void)
+{
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        rs00_mit_send_clear_fault(GUGUJI_JOINTS[i].can_id);
+    }
+}
+
+static void enable_all_motors(void)
+{
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        // RS00 MIT 协议中 0 表示 MIT 运控模式；上电默认也是 MIT，这里再写一次便于调试。
+        rs00_mit_send_set_mode(GUGUJI_JOINTS[i].can_id, 0);
+        vTaskDelay(pdMS_TO_TICKS(2));
+        rs00_mit_send_enable(GUGUJI_JOINTS[i].can_id);
+        vTaskDelay(pdMS_TO_TICKS(2));
+    }
+}
+
+#if !GUGUJI_WIFI_USE_AP
+static void wifi_event_handler(
+    void *arg,
+    esp_event_base_t event_base,
+    int32_t event_id,
+    void *event_data)
+{
+    (void)arg;
+    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
+        esp_wifi_connect();
+    } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
+        ESP_LOGW(TAG, "WiFi 断开，尝试重连");
+        esp_wifi_connect();
+        xEventGroupClearBits(g_wifi_event_group, WIFI_CONNECTED_BIT);
+    } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
+        ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
+        ESP_LOGI(TAG, "WiFi 已连接，IP=" IPSTR, IP2STR(&event->ip_info.ip));
+        xEventGroupSetBits(g_wifi_event_group, WIFI_CONNECTED_BIT);
+    }
+}
+#endif
+
+static void wifi_init(void)
+{
+    ESP_ERROR_CHECK(esp_netif_init());
+    ESP_ERROR_CHECK(esp_event_loop_create_default());
+
+    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
+
+#if GUGUJI_WIFI_USE_AP
+    esp_netif_create_default_wifi_ap();
+    wifi_config_t wifi_config = {
+        .ap = {
+            .ssid = GUGUJI_WIFI_AP_SSID,
+            .ssid_len = strlen(GUGUJI_WIFI_AP_SSID),
+            .password = GUGUJI_WIFI_AP_PASSWORD,
+            .channel = 6,
+            .max_connection = 2,
+            .authmode = WIFI_AUTH_WPA_WPA2_PSK,
+        },
+    };
+    if (strlen(GUGUJI_WIFI_AP_PASSWORD) == 0) {
+        wifi_config.ap.authmode = WIFI_AUTH_OPEN;
+    }
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
+    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config));
+    ESP_ERROR_CHECK(esp_wifi_start());
+    ESP_LOGI(TAG, "WiFi AP 已启动: ssid=%s", GUGUJI_WIFI_AP_SSID);
+#else
+    g_wifi_event_group = xEventGroupCreate();
+    esp_netif_create_default_wifi_sta();
+    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, NULL));
+    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL, NULL));
+    wifi_config_t wifi_config = {
+        .sta = {
+            .ssid = GUGUJI_WIFI_STA_SSID,
+            .password = GUGUJI_WIFI_STA_PASSWORD,
+        },
+    };
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
+    ESP_ERROR_CHECK(esp_wifi_start());
+    xEventGroupWaitBits(g_wifi_event_group, WIFI_CONNECTED_BIT, pdFALSE, pdTRUE, portMAX_DELAY);
+#endif
+}
+
+static void udp_receive_task(void *arg)
+{
+    (void)arg;
+    bool peer_logged = false;
+    uint32_t last_logged_flags = UINT32_MAX;
+    const int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
+    if (sock < 0) {
+        ESP_LOGE(TAG, "创建 UDP socket 失败: errno=%d", errno);
+        vTaskDelete(NULL);
+    }
+
+    struct sockaddr_in listen_addr = {
+        .sin_family = AF_INET,
+        .sin_port = htons(GUGUJI_UDP_COMMAND_PORT),
+        .sin_addr.s_addr = htonl(INADDR_ANY),
+    };
+    if (bind(sock, (struct sockaddr *)&listen_addr, sizeof(listen_addr)) < 0) {
+        ESP_LOGE(TAG, "绑定 UDP 端口失败: errno=%d", errno);
+        close(sock);
+        vTaskDelete(NULL);
+    }
+    ESP_LOGI(TAG, "UDP 命令端口已监听: %d", GUGUJI_UDP_COMMAND_PORT);
+
+    while (true) {
+        guguji_command_packet_t packet = {0};
+        struct sockaddr_storage source_addr = {0};
+        socklen_t source_len = sizeof(source_addr);
+        const int received = recvfrom(
+            sock,
+            &packet,
+            sizeof(packet),
+            0,
+            (struct sockaddr *)&source_addr,
+            &source_len);
+        if (received < 0) {
+            continue;
+        }
+        if (!guguji_validate_command_packet(&packet, (size_t)received)) {
+            ESP_LOGW(TAG, "收到无效 UDP 命令包，长度=%d", received);
+            continue;
+        }
+
+        xSemaphoreTake(g_command_mutex, portMAX_DELAY);
+        memcpy(&g_latest_command, &packet, sizeof(packet));
+        g_have_command = true;
+        g_last_command_ms = now_ms();
+        g_latest_command_sequence = packet.sequence;
+        xSemaphoreGive(g_command_mutex);
+
+        xSemaphoreTake(g_peer_mutex, portMAX_DELAY);
+        memcpy(&g_last_peer_addr, &source_addr, source_len);
+        g_last_peer_len = source_len;
+        g_have_peer = true;
+        xSemaphoreGive(g_peer_mutex);
+
+        if (!peer_logged && source_addr.ss_family == AF_INET) {
+            char addr_str[INET_ADDRSTRLEN] = {0};
+            const struct sockaddr_in *source_ipv4 = (const struct sockaddr_in *)&source_addr;
+            inet_ntoa_r(source_ipv4->sin_addr, addr_str, sizeof(addr_str));
+            ESP_LOGI(TAG, "收到主机命令: %s:%d", addr_str, ntohs(source_ipv4->sin_port));
+            peer_logged = true;
+        }
+        if (packet.flags != last_logged_flags) {
+            ESP_LOGI(TAG, "主机命令 flags=0x%08" PRIX32 " seq=%" PRIu32, packet.flags, packet.sequence);
+            last_logged_flags = packet.flags;
+        }
+    }
+}
+
+static void can_feedback_task(void *arg)
+{
+    (void)arg;
+    while (true) {
+        rs00_feedback_t feedback = {0};
+        if (!rs00_mit_poll_feedback(&feedback, 10)) {
+            continue;
+        }
+        const int joint_index = find_joint_by_motor_id(feedback.motor_id);
+        if (joint_index < 0) {
+            continue;
+        }
+
+        xSemaphoreTake(g_feedback_mutex, portMAX_DELAY);
+        g_feedback[joint_index] = feedback;
+        xSemaphoreGive(g_feedback_mutex);
+    }
+}
+
+static void sensor_task(void *arg)
+{
+    (void)arg;
+    uint32_t last_ms = now_ms();
+    while (true) {
+        const uint32_t current_ms = now_ms();
+        const float dt_s = (float)(current_ms - last_ms) * 0.001f;
+        last_ms = current_ms;
+
+        xSemaphoreTake(g_sensor_mutex, portMAX_DELAY);
+        guguji_sensors_update(&g_sensor_state, dt_s > 0.0f ? dt_s : 0.02f);
+        xSemaphoreGive(g_sensor_mutex);
+
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+}
+
+static void control_task(void *arg)
+{
+    (void)arg;
+    bool motors_enabled = false;
+    uint32_t last_clear_fault_sequence = UINT32_MAX;
+
+    while (true) {
+        guguji_command_packet_t command = {0};
+        bool have_command = false;
+        uint32_t command_age_ms = UINT32_MAX;
+
+        xSemaphoreTake(g_command_mutex, portMAX_DELAY);
+        have_command = g_have_command;
+        if (have_command) {
+            command = g_latest_command;
+            command_age_ms = now_ms() - g_last_command_ms;
+        }
+        xSemaphoreGive(g_command_mutex);
+
+        if (!have_command || command_age_ms > GUGUJI_COMMAND_TIMEOUT_MS) {
+            if (motors_enabled) {
+                stop_all_motors();
+                motors_enabled = false;
+            }
+            g_robot_state = have_command ? GUGUJI_STATE_TIMEOUT : GUGUJI_STATE_DISARMED;
+            vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
+            continue;
+        }
+
+        if ((command.flags & GUGUJI_COMMAND_FLAG_CLEAR_FAULTS) != 0 &&
+            command.sequence != last_clear_fault_sequence) {
+            clear_all_motor_faults();
+            last_clear_fault_sequence = command.sequence;
+        }
+
+        if ((command.flags & GUGUJI_COMMAND_FLAG_ESTOP) != 0) {
+            stop_all_motors();
+            motors_enabled = false;
+            g_robot_state = GUGUJI_STATE_ESTOP;
+            vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
+            continue;
+        }
+
+        if ((command.flags & GUGUJI_COMMAND_FLAG_DISARM) != 0 ||
+            (command.flags & GUGUJI_COMMAND_FLAG_ARM) == 0) {
+            if (motors_enabled) {
+                stop_all_motors();
+                motors_enabled = false;
+            }
+            g_robot_state = GUGUJI_STATE_DISARMED;
+            vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
+            continue;
+        }
+
+        if (!motors_enabled) {
+            enable_all_motors();
+            motors_enabled = true;
+            ESP_LOGI(TAG, "电机已使能");
+        }
+
+        g_robot_state = GUGUJI_STATE_ARMED;
+        for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+            const guguji_joint_config_t *joint = &GUGUJI_JOINTS[i];
+            const float joint_target = clampf_local(
+                command.positions_rad[i],
+                joint->lower_limit_rad,
+                joint->upper_limit_rad);
+            const float motor_position = joint_to_motor_angle(i, joint_target);
+            const float motor_velocity = command.velocities_rad_s[i] * joint->direction;
+            const float motor_torque = command.torque_ff_nm[i] * joint->direction;
+            rs00_mit_send_control(
+                joint->can_id,
+                motor_position,
+                motor_velocity,
+                command.kp[i],
+                command.kd[i],
+                motor_torque);
+        }
+
+        vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
+    }
+}
+
+static void telemetry_task(void *arg)
+{
+    (void)arg;
+    const int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
+    if (sock < 0) {
+        ESP_LOGE(TAG, "创建遥测 UDP socket 失败: errno=%d", errno);
+        vTaskDelete(NULL);
+    }
+
+    while (true) {
+        struct sockaddr_storage peer_addr = {0};
+        socklen_t peer_len = 0;
+        bool have_peer = false;
+
+        xSemaphoreTake(g_peer_mutex, portMAX_DELAY);
+        have_peer = g_have_peer;
+        if (have_peer) {
+            memcpy(&peer_addr, &g_last_peer_addr, g_last_peer_len);
+            peer_len = g_last_peer_len;
+        }
+        xSemaphoreGive(g_peer_mutex);
+
+        if (!have_peer) {
+            vTaskDelay(pdMS_TO_TICKS(GUGUJI_TELEMETRY_PERIOD_MS));
+            continue;
+        }
+
+        guguji_telemetry_packet_t packet = {0};
+        packet.robot_state = (uint8_t)g_robot_state;
+        packet.sequence = g_latest_command_sequence;
+        packet.uptime_ms = now_ms();
+
+        xSemaphoreTake(g_command_mutex, portMAX_DELAY);
+        packet.last_command_age_ms = g_have_command ? (now_ms() - g_last_command_ms) : UINT32_MAX;
+        xSemaphoreGive(g_command_mutex);
+
+        uint32_t fault_mask = 0;
+        const uint32_t telemetry_now_ms = packet.uptime_ms;
+        xSemaphoreTake(g_feedback_mutex, portMAX_DELAY);
+        for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+            const rs00_feedback_t feedback = g_feedback[i];
+            packet.joint_position_rad[i] = motor_to_joint_angle(i, feedback.position_rad);
+            packet.joint_velocity_rad_s[i] = feedback.velocity_rad_s * GUGUJI_JOINTS[i].direction;
+            packet.joint_torque_nm[i] = feedback.torque_nm * GUGUJI_JOINTS[i].direction;
+            packet.motor_temperature_c[i] = feedback.temperature_c;
+            if (feedback.fault) {
+                fault_mask |= (1u << i);
+            }
+            if (feedback.last_feedback_ms == 0 ||
+                telemetry_now_ms - feedback.last_feedback_ms > GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS) {
+                fault_mask |= (1u << (8 + i));
+            }
+        }
+        xSemaphoreGive(g_feedback_mutex);
+
+        xSemaphoreTake(g_sensor_mutex, portMAX_DELAY);
+        memcpy(packet.accel_m_s2, g_sensor_state.accel_m_s2, sizeof(packet.accel_m_s2));
+        memcpy(packet.gyro_rad_s, g_sensor_state.gyro_rad_s, sizeof(packet.gyro_rad_s));
+        memcpy(packet.mag_u_t, g_sensor_state.mag_u_t, sizeof(packet.mag_u_t));
+        memcpy(packet.rpy_rad, g_sensor_state.rpy_rad, sizeof(packet.rpy_rad));
+        if (!g_sensor_state.imu_ok) {
+            fault_mask |= GUGUJI_FAULT_IMU_OFFLINE;
+        }
+        if (!g_sensor_state.mag_ok) {
+            fault_mask |= GUGUJI_FAULT_MAG_OFFLINE;
+        }
+        xSemaphoreGive(g_sensor_mutex);
+
+        g_latest_fault_mask = fault_mask;
+        packet.fault_mask = fault_mask;
+
+        guguji_finalize_telemetry_packet(&packet);
+        sendto(sock, &packet, sizeof(packet), 0, (struct sockaddr *)&peer_addr, peer_len);
+        vTaskDelay(pdMS_TO_TICKS(GUGUJI_TELEMETRY_PERIOD_MS));
+    }
+}
+
+static void status_task(void *arg)
+{
+    (void)arg;
+    while (true) {
+        bool have_peer = false;
+        xSemaphoreTake(g_peer_mutex, portMAX_DELAY);
+        have_peer = g_have_peer;
+        xSemaphoreGive(g_peer_mutex);
+
+        const uint32_t fault_mask = g_latest_fault_mask;
+        const uint32_t motor_faults = fault_mask & GUGUJI_FAULT_MOTOR_MASK;
+        const uint32_t warning_faults = fault_mask & (
+            GUGUJI_FAULT_FEEDBACK_TIMEOUT_MASK |
+            GUGUJI_FAULT_IMU_OFFLINE |
+            GUGUJI_FAULT_MAG_OFFLINE);
+        guguji_status_mode_t mode = GUGUJI_STATUS_DISARMED;
+
+        if (motor_faults != 0) {
+            mode = GUGUJI_STATUS_FAULT;
+        } else if (g_robot_state == GUGUJI_STATE_ESTOP) {
+            mode = GUGUJI_STATUS_ESTOP;
+        } else if (g_robot_state == GUGUJI_STATE_TIMEOUT) {
+            mode = GUGUJI_STATUS_TIMEOUT;
+        } else if (!have_peer) {
+            mode = GUGUJI_STATUS_WAITING_HOST;
+        } else if (g_robot_state == GUGUJI_STATE_ARMED) {
+            mode = GUGUJI_STATUS_ARMED;
+        } else if (warning_faults != 0) {
+            mode = GUGUJI_STATUS_SENSOR_WARN;
+        } else {
+            mode = GUGUJI_STATUS_DISARMED;
+        }
+
+        guguji_status_outputs_apply(mode, now_ms());
+        vTaskDelay(pdMS_TO_TICKS(80));
+    }
+}
+
+void app_main(void)
+{
+    esp_err_t err = nvs_flash_init();
+    if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+        ESP_ERROR_CHECK(nvs_flash_erase());
+        ESP_ERROR_CHECK(nvs_flash_init());
+    } else {
+        ESP_ERROR_CHECK(err);
+    }
+
+    g_command_mutex = xSemaphoreCreateMutex();
+    g_feedback_mutex = xSemaphoreCreateMutex();
+    g_sensor_mutex = xSemaphoreCreateMutex();
+    g_peer_mutex = xSemaphoreCreateMutex();
+    memset(&g_sensor_state, 0, sizeof(g_sensor_state));
+    memset(&g_feedback, 0, sizeof(g_feedback));
+
+    ESP_ERROR_CHECK(guguji_status_outputs_init());
+    wifi_init();
+    ESP_ERROR_CHECK(guguji_sensors_init());
+    ESP_ERROR_CHECK(rs00_mit_init(GUGUJI_TWAI_TX_GPIO, GUGUJI_TWAI_RX_GPIO));
+
+    xTaskCreatePinnedToCore(udp_receive_task, "udp_rx", 4096, NULL, 6, NULL, 0);
+    xTaskCreatePinnedToCore(control_task, "control", 4096, NULL, 8, NULL, 1);
+    xTaskCreatePinnedToCore(can_feedback_task, "can_rx", 4096, NULL, 7, NULL, 1);
+    xTaskCreatePinnedToCore(sensor_task, "sensors", 4096, NULL, 5, NULL, 0);
+    xTaskCreatePinnedToCore(telemetry_task, "telemetry", 4096, NULL, 5, NULL, 0);
+    xTaskCreatePinnedToCore(status_task, "status", 4096, NULL, 4, NULL, 0);
+
+    ESP_LOGI(TAG, "guguji ESP32S3 real-robot bridge started");
+}

guguji_real_robot/firmware/esp32s3_espidf/main/config.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "driver/gpio.h"
+
+// ==============================
+// WiFi 配置
+// ==============================
+// 第一版默认让 ESP32S3 自己开热点，电脑连接 guguji-robot 后即可直连。
+// 如果你希望接入现有路由器，把 GUGUJI_WIFI_USE_AP 改成 0，并填写 STA SSID/PASSWORD。
+#define GUGUJI_WIFI_USE_AP 1
+#define GUGUJI_WIFI_AP_SSID "guguji-robot"
+#define GUGUJI_WIFI_AP_PASSWORD "guguji1234"
+#define GUGUJI_WIFI_STA_SSID "your-router-ssid"
+#define GUGUJI_WIFI_STA_PASSWORD "your-router-password"
+
+#define GUGUJI_UDP_COMMAND_PORT 7777
+// ==============================
+// ESP32S3 引脚配置
+// ==============================
+// 控制板上已经集成 TJA1050T CAN 总线收发器，下面两个引脚连接到收发器 TXD/RXD。
+#define GUGUJI_TWAI_TX_GPIO GPIO_NUM_5
+#define GUGUJI_TWAI_RX_GPIO GPIO_NUM_4
+
+// 无屏状态提示硬件。
+#define GUGUJI_BUZZER_GPIO GPIO_NUM_42
+#define GUGUJI_WS2812_GPIO GPIO_NUM_45
+#define GUGUJI_WS2812_RMT_CHANNEL RMT_CHANNEL_0
+
+#define GUGUJI_I2C_PORT I2C_NUM_0
+#define GUGUJI_I2C_SDA_GPIO GPIO_NUM_8
+#define GUGUJI_I2C_SCL_GPIO GPIO_NUM_9
+#define GUGUJI_I2C_FREQ_HZ 400000
+
+// QMI8658A 常见地址为 0x6A 或 0x6B；若初始化失败，先改这里。
+#define GUGUJI_QMI8658A_ADDR 0x6B
+// QMC6309 数据手册给出的 7-bit 地址为 0x7C。
+#define GUGUJI_QMC6309_ADDR 0x7C
+
+// ==============================
+// 控制周期和安全参数
+// ==============================
+#define GUGUJI_CONTROL_PERIOD_MS 20
+#define GUGUJI_TELEMETRY_PERIOD_MS 20
+#define GUGUJI_COMMAND_TIMEOUT_MS 200
+#define GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS 500

guguji_real_robot/firmware/esp32s3_espidf/main/guguji_protocol.c

@@ -0,0 +1,55 @@
+#include "guguji_protocol.h"
+
+#include <math.h>
+
+uint32_t guguji_crc32(const uint8_t *data, size_t length)
+{
+    uint32_t crc = 0xFFFFFFFFu;
+    for (size_t i = 0; i < length; ++i) {
+        crc ^= data[i];
+        for (int bit = 0; bit < 8; ++bit) {
+            const uint32_t mask = -(crc & 1u);
+            crc = (crc >> 1) ^ (0xEDB88320u & mask);
+        }
+    }
+    return ~crc;
+}
+
+bool guguji_validate_command_packet(const guguji_command_packet_t *packet, size_t length)
+{
+    if (packet == NULL || length != sizeof(guguji_command_packet_t)) {
+        return false;
+    }
+    if (packet->magic != GUGUJI_COMMAND_MAGIC ||
+        packet->version != GUGUJI_PROTOCOL_VERSION ||
+        packet->command_type != GUGUJI_COMMAND_TYPE_SETPOINTS ||
+        packet->joint_count != GUGUJI_JOINT_COUNT) {
+        return false;
+    }
+
+    const size_t crc_offset = sizeof(guguji_command_packet_t) - sizeof(uint32_t);
+    const uint32_t expected_crc = guguji_crc32((const uint8_t *)packet, crc_offset);
+    if (expected_crc != packet->crc32) {
+        return false;
+    }
+
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        if (!isfinite(packet->positions_rad[i]) ||
+            !isfinite(packet->velocities_rad_s[i]) ||
+            !isfinite(packet->kp[i]) ||
+            !isfinite(packet->kd[i]) ||
+            !isfinite(packet->torque_ff_nm[i])) {
+            return false;
+        }
+    }
+    return true;
+}
+
+void guguji_finalize_telemetry_packet(guguji_telemetry_packet_t *packet)
+{
+    packet->magic = GUGUJI_TELEMETRY_MAGIC;
+    packet->version = GUGUJI_PROTOCOL_VERSION;
+    packet->joint_count = GUGUJI_JOINT_COUNT;
+    const size_t crc_offset = sizeof(guguji_telemetry_packet_t) - sizeof(uint32_t);
+    packet->crc32 = guguji_crc32((const uint8_t *)packet, crc_offset);
+}

guguji_real_robot/firmware/esp32s3_espidf/main/guguji_protocol.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#define GUGUJI_JOINT_COUNT 8
+#define GUGUJI_PROTOCOL_VERSION 1
+
+#define GUGUJI_COMMAND_MAGIC 0x43475547u
+#define GUGUJI_TELEMETRY_MAGIC 0x54475547u
+
+#define GUGUJI_COMMAND_TYPE_SETPOINTS 1
+
+#define GUGUJI_COMMAND_FLAG_ARM (1u << 0)
+#define GUGUJI_COMMAND_FLAG_DISARM (1u << 1)
+#define GUGUJI_COMMAND_FLAG_HOLD (1u << 2)
+#define GUGUJI_COMMAND_FLAG_ESTOP (1u << 3)
+#define GUGUJI_COMMAND_FLAG_CLEAR_FAULTS (1u << 4)
+
+#define GUGUJI_FAULT_MOTOR_MASK 0x000000FFu
+#define GUGUJI_FAULT_FEEDBACK_TIMEOUT_MASK 0x0000FF00u
+#define GUGUJI_FAULT_IMU_OFFLINE (1u << 16)
+#define GUGUJI_FAULT_MAG_OFFLINE (1u << 17)
+
+typedef enum {
+    GUGUJI_STATE_DISARMED = 0,
+    GUGUJI_STATE_ARMED = 1,
+    GUGUJI_STATE_ESTOP = 2,
+    GUGUJI_STATE_TIMEOUT = 3,
+} guguji_robot_state_t;
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;
+    uint8_t version;
+    uint8_t command_type;
+    uint16_t joint_count;
+    uint32_t sequence;
+    uint32_t monotonic_ms;
+    uint32_t flags;
+    float positions_rad[GUGUJI_JOINT_COUNT];
+    float velocities_rad_s[GUGUJI_JOINT_COUNT];
+    float kp[GUGUJI_JOINT_COUNT];
+    float kd[GUGUJI_JOINT_COUNT];
+    float torque_ff_nm[GUGUJI_JOINT_COUNT];
+    uint32_t crc32;
+} guguji_command_packet_t;
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;
+    uint8_t version;
+    uint8_t robot_state;
+    uint16_t joint_count;
+    uint32_t sequence;
+    uint32_t uptime_ms;
+    uint32_t last_command_age_ms;
+    uint32_t fault_mask;
+    float joint_position_rad[GUGUJI_JOINT_COUNT];
+    float joint_velocity_rad_s[GUGUJI_JOINT_COUNT];
+    float joint_torque_nm[GUGUJI_JOINT_COUNT];
+    float motor_temperature_c[GUGUJI_JOINT_COUNT];
+    float accel_m_s2[3];
+    float gyro_rad_s[3];
+    float mag_u_t[3];
+    float rpy_rad[3];
+    uint32_t crc32;
+} guguji_telemetry_packet_t;
+
+uint32_t guguji_crc32(const uint8_t *data, size_t length);
+bool guguji_validate_command_packet(const guguji_command_packet_t *packet, size_t length);
+void guguji_finalize_telemetry_packet(guguji_telemetry_packet_t *packet);

guguji_real_robot/firmware/esp32s3_espidf/main/robot_config.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "guguji_protocol.h"
+
+typedef struct {
+    const char *name;
+    uint8_t can_id;
+    float direction;
+    float zero_offset_rad;
+    float lower_limit_rad;
+    float upper_limit_rad;
+} guguji_joint_config_t;
+
+// 这里的顺序必须和训练配置中的 joint_names 完全一致。
+// direction / zero_offset_rad 用于把“仿真关节角”映射到“电机机械角”：
+// motor_angle = joint_angle * direction + zero_offset_rad。
+// 真机第一次测试时务必悬空，逐个关节确认正方向后再修改 direction。
+static const guguji_joint_config_t GUGUJI_JOINTS[GUGUJI_JOINT_COUNT] = {
+    {"left_hip_pitch_joint", 1, 1.0f, 0.0f, -1.0f, 1.0f},
+    {"left_knee_pitch_joint", 2, 1.0f, 0.0f, -1.2f, 1.2f},
+    {"left_ankle_pitch_joint", 3, 1.0f, 0.0f, -0.8f, 0.8f},
+    {"left_ankle_joint", 4, 1.0f, 0.0f, -0.6f, 0.6f},
+    {"right_hip_pitch_joint", 5, 1.0f, 0.0f, -1.0f, 1.0f},
+    {"right_knee_pitch_joint", 6, 1.0f, 0.0f, -1.2f, 1.2f},
+    {"right_ankle_pitch_joint", 7, 1.0f, 0.0f, -0.8f, 0.8f},
+    {"right_ankle_joint", 8, 1.0f, 0.0f, -0.6f, 0.6f},
+};

guguji_real_robot/firmware/esp32s3_espidf/main/rs00_mit.c

@@ -0,0 +1,142 @@
+#include "rs00_mit.h"
+
+#include <math.h>
+#include <string.h>
+
+#include "driver/twai.h"
+#include "esp_timer.h"
+
+static float clampf(float value, float min_value, float max_value)
+{
+    if (value < min_value) {
+        return min_value;
+    }
+    if (value > max_value) {
+        return max_value;
+    }
+    return value;
+}
+
+static uint32_t float_to_uint(float value, float min_value, float max_value, uint8_t bits)
+{
+    const float clipped = clampf(value, min_value, max_value);
+    const float span = max_value - min_value;
+    const uint32_t max_int = (1u << bits) - 1u;
+    return (uint32_t)((clipped - min_value) * (float)max_int / span + 0.5f);
+}
+
+static float uint_to_float(uint32_t value, float min_value, float max_value, uint8_t bits)
+{
+    const float span = max_value - min_value;
+    const uint32_t max_int = (1u << bits) - 1u;
+    return ((float)value) * span / (float)max_int + min_value;
+}
+
+static esp_err_t transmit_standard(uint32_t identifier, const uint8_t data[8])
+{
+    twai_message_t message = {
+        .identifier = identifier,
+        .data_length_code = 8,
+        .flags = 0,
+    };
+    memcpy(message.data, data, 8);
+    return twai_transmit(&message, pdMS_TO_TICKS(3));
+}
+
+esp_err_t rs00_mit_init(gpio_num_t tx_gpio, gpio_num_t rx_gpio)
+{
+    twai_general_config_t general_config = TWAI_GENERAL_CONFIG_DEFAULT(tx_gpio, rx_gpio, TWAI_MODE_NORMAL);
+    general_config.tx_queue_len = 32;
+    general_config.rx_queue_len = 32;
+    twai_timing_config_t timing_config = TWAI_TIMING_CONFIG_1MBITS();
+    twai_filter_config_t filter_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
+
+    esp_err_t err = twai_driver_install(&general_config, &timing_config, &filter_config);
+    if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
+        return err;
+    }
+    err = twai_start();
+    if (err == ESP_ERR_INVALID_STATE) {
+        return ESP_OK;
+    }
+    return err;
+}
+
+esp_err_t rs00_mit_send_enable(uint8_t motor_id)
+{
+    const uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};
+    return transmit_standard(motor_id, data);
+}
+
+esp_err_t rs00_mit_send_stop(uint8_t motor_id)
+{
+    const uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};
+    return transmit_standard(motor_id, data);
+}
+
+esp_err_t rs00_mit_send_clear_fault(uint8_t motor_id)
+{
+    const uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB};
+    return transmit_standard(motor_id, data);
+}
+
+esp_err_t rs00_mit_send_set_mode(uint8_t motor_id, uint8_t mode)
+{
+    uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, mode, 0xFC};
+    return transmit_standard(motor_id, data);
+}
+
+esp_err_t rs00_mit_send_control(
+    uint8_t motor_id,
+    float position_rad,
+    float velocity_rad_s,
+    float kp,
+    float kd,
+    float torque_ff_nm)
+{
+    const uint32_t p = float_to_uint(position_rad, -12.57f, 12.57f, 16);
+    const uint32_t v = float_to_uint(velocity_rad_s, -33.0f, 33.0f, 12);
+    const uint32_t kp_u = float_to_uint(kp, 0.0f, 500.0f, 12);
+    const uint32_t kd_u = float_to_uint(kd, 0.0f, 5.0f, 12);
+    const uint32_t t = float_to_uint(torque_ff_nm, -14.0f, 14.0f, 12);
+
+    uint8_t data[8] = {
+        (uint8_t)(p >> 8),
+        (uint8_t)(p & 0xFFu),
+        (uint8_t)(v >> 4),
+        (uint8_t)(((v & 0x0Fu) << 4) | (kp_u >> 8)),
+        (uint8_t)(kp_u & 0xFFu),
+        (uint8_t)(kd_u >> 4),
+        (uint8_t)(((kd_u & 0x0Fu) << 4) | (t >> 8)),
+        (uint8_t)(t & 0xFFu),
+    };
+    return transmit_standard(motor_id, data);
+}
+
+bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms)
+{
+    twai_message_t message = {0};
+    if (twai_receive(&message, pdMS_TO_TICKS(timeout_ms)) != ESP_OK) {
+        return false;
+    }
+    if ((message.flags & TWAI_MSG_FLAG_EXTD) != 0 || message.data_length_code != 8) {
+        return false;
+    }
+
+    const uint8_t *data = message.data;
+    const uint32_t p = ((uint32_t)data[1] << 8) | data[2];
+    const uint32_t v = ((uint32_t)data[3] << 4) | (data[4] >> 4);
+    const uint32_t t = ((uint32_t)(data[4] & 0x0F) << 8) | data[5];
+    const uint32_t temp_raw = ((uint32_t)(data[6] & 0x0F) << 8) | data[7];
+
+    feedback->motor_id = data[0];
+    feedback->position_rad = uint_to_float(p, -12.57f, 12.57f, 16);
+    feedback->velocity_rad_s = uint_to_float(v, -33.0f, 33.0f, 12);
+    feedback->torque_nm = uint_to_float(t, -14.0f, 14.0f, 12);
+    feedback->mode_state = (data[6] >> 6) & 0x03u;
+    feedback->fault = (data[6] & 0x20u) != 0;
+    feedback->warning = (data[6] & 0x10u) != 0;
+    feedback->temperature_c = (float)temp_raw * 0.1f;
+    feedback->last_feedback_ms = (uint32_t)(esp_timer_get_time() / 1000ULL);
+    return true;
+}

guguji_real_robot/firmware/esp32s3_espidf/main/rs00_mit.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "driver/gpio.h"
+#include "esp_err.h"
+
+typedef struct {
+    uint8_t motor_id;
+    float position_rad;
+    float velocity_rad_s;
+    float torque_nm;
+    float temperature_c;
+    uint8_t mode_state;
+    bool fault;
+    bool warning;
+    uint32_t last_feedback_ms;
+} rs00_feedback_t;
+
+esp_err_t rs00_mit_init(gpio_num_t tx_gpio, gpio_num_t rx_gpio);
+esp_err_t rs00_mit_send_enable(uint8_t motor_id);
+esp_err_t rs00_mit_send_stop(uint8_t motor_id);
+esp_err_t rs00_mit_send_clear_fault(uint8_t motor_id);
+esp_err_t rs00_mit_send_set_mode(uint8_t motor_id, uint8_t mode);
+esp_err_t rs00_mit_send_control(
+    uint8_t motor_id,
+    float position_rad,
+    float velocity_rad_s,
+    float kp,
+    float kd,
+    float torque_ff_nm);
+bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms);

guguji_real_robot/firmware/esp32s3_espidf/main/sensors.c

@@ -0,0 +1,166 @@
+#include "sensors.h"
+
+#include <math.h>
+#include <string.h>
+
+#include "config.h"
+#include "driver/i2c.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+static const char *TAG = "guguji_sensors";
+
+#define QMI_REG_WHO_AM_I 0x00
+#define QMI_REG_CTRL1 0x02
+#define QMI_REG_CTRL2 0x03
+#define QMI_REG_CTRL3 0x04
+#define QMI_REG_CTRL5 0x06
+#define QMI_REG_CTRL7 0x08
+#define QMI_REG_DATA_START 0x35
+
+#define QMC_REG_CHIP_ID 0x00
+#define QMC_REG_DATA_START 0x01
+#define QMC_REG_STATUS 0x09
+#define QMC_REG_CTRL1 0x0A
+#define QMC_REG_CTRL2 0x0B
+
+static bool g_qmi_ok = false;
+static bool g_qmc_ok = false;
+
+static esp_err_t i2c_write_u8(uint8_t addr, uint8_t reg, uint8_t value)
+{
+    uint8_t payload[2] = {reg, value};
+    return i2c_master_write_to_device(GUGUJI_I2C_PORT, addr, payload, sizeof(payload), pdMS_TO_TICKS(50));
+}
+
+static esp_err_t i2c_read(uint8_t addr, uint8_t reg, uint8_t *data, size_t length)
+{
+    return i2c_master_write_read_device(GUGUJI_I2C_PORT, addr, &reg, 1, data, length, pdMS_TO_TICKS(50));
+}
+
+static int16_t le_i16(const uint8_t *data)
+{
+    return (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8));
+}
+
+esp_err_t guguji_sensors_init(void)
+{
+    i2c_config_t config = {
+        .mode = I2C_MODE_MASTER,
+        .sda_io_num = GUGUJI_I2C_SDA_GPIO,
+        .scl_io_num = GUGUJI_I2C_SCL_GPIO,
+        .sda_pullup_en = GPIO_PULLUP_ENABLE,
+        .scl_pullup_en = GPIO_PULLUP_ENABLE,
+        .master.clk_speed = GUGUJI_I2C_FREQ_HZ,
+        .clk_flags = 0,
+    };
+    ESP_ERROR_CHECK(i2c_param_config(GUGUJI_I2C_PORT, &config));
+    ESP_ERROR_CHECK(i2c_driver_install(GUGUJI_I2C_PORT, config.mode, 0, 0, 0));
+
+    uint8_t who_am_i = 0;
+    if (i2c_read(GUGUJI_QMI8658A_ADDR, QMI_REG_WHO_AM_I, &who_am_i, 1) == ESP_OK && who_am_i == 0x05) {
+        // CTRL1: 开启地址自增并使用 little-endian，方便一次读完 AX~GZ。
+        i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL1, 0x40);
+        // CTRL2: 加速度 ±8g，ODR 约 112Hz；CTRL3: 陀螺仪 ±512dps，ODR 约 112Hz。
+        i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL2, 0x26);
+        i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL3, 0x56);
+        // 打开加速度和陀螺仪低通滤波。
+        i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL5, 0x11);
+        // 使能 accel + gyro。
+        i2c_write_u8(GUGUJI_QMI8658A_ADDR, QMI_REG_CTRL7, 0x03);
+        g_qmi_ok = true;
+        ESP_LOGI(TAG, "QMI8658A 初始化成功");
+    } else {
+        ESP_LOGW(TAG, "QMI8658A 初始化失败，who_am_i=0x%02X", who_am_i);
+    }
+
+    uint8_t chip_id = 0;
+    if (i2c_read(GUGUJI_QMC6309_ADDR, QMC_REG_CHIP_ID, &chip_id, 1) == ESP_OK && chip_id == 0x90) {
+        // 数据手册推荐的连续测量配置：32Gauss 量程、Set/Reset 开、OSR1/OSR2=8。
+        i2c_write_u8(GUGUJI_QMC6309_ADDR, QMC_REG_CTRL2, 0x00);
+        i2c_write_u8(GUGUJI_QMC6309_ADDR, QMC_REG_CTRL1, 0x63);
+        g_qmc_ok = true;
+        ESP_LOGI(TAG, "QMC6309 初始化成功");
+    } else {
+        ESP_LOGW(TAG, "QMC6309 初始化失败，chip_id=0x%02X", chip_id);
+    }
+
+    return ESP_OK;
+}
+
+static void update_imu(guguji_sensor_state_t *state)
+{
+    uint8_t raw[12] = {0};
+    state->imu_ok = false;
+    if (!g_qmi_ok || i2c_read(GUGUJI_QMI8658A_ADDR, QMI_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
+        return;
+    }
+
+    const int16_t ax = le_i16(&raw[0]);
+    const int16_t ay = le_i16(&raw[2]);
+    const int16_t az = le_i16(&raw[4]);
+    const int16_t gx = le_i16(&raw[6]);
+    const int16_t gy = le_i16(&raw[8]);
+    const int16_t gz = le_i16(&raw[10]);
+
+    const float accel_scale = 9.80665f / 4096.0f;       // ±8g
+    const float gyro_scale = 3.14159265358979323846f / 180.0f / 64.0f; // ±512dps
+
+    state->accel_m_s2[0] = (float)ax * accel_scale;
+    state->accel_m_s2[1] = (float)ay * accel_scale;
+    state->accel_m_s2[2] = (float)az * accel_scale;
+    state->gyro_rad_s[0] = (float)gx * gyro_scale;
+    state->gyro_rad_s[1] = (float)gy * gyro_scale;
+    state->gyro_rad_s[2] = (float)gz * gyro_scale;
+    state->imu_ok = true;
+}
+
+static void update_mag(guguji_sensor_state_t *state)
+{
+    uint8_t status = 0;
+    state->mag_ok = false;
+    if (!g_qmc_ok || i2c_read(GUGUJI_QMC6309_ADDR, QMC_REG_STATUS, &status, 1) != ESP_OK) {
+        return;
+    }
+    if ((status & 0x01u) == 0) {
+        return;
+    }
+
+    uint8_t raw[6] = {0};
+    if (i2c_read(GUGUJI_QMC6309_ADDR, QMC_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
+        return;
+    }
+    // QMC6309 典型分辨率约 1mGs/LSB，即 0.1uT/LSB。
+    state->mag_u_t[0] = (float)le_i16(&raw[0]) * 0.1f;
+    state->mag_u_t[1] = (float)le_i16(&raw[2]) * 0.1f;
+    state->mag_u_t[2] = (float)le_i16(&raw[4]) * 0.1f;
+    state->mag_ok = true;
+}
+
+void guguji_sensors_update(guguji_sensor_state_t *state, float dt_s)
+{
+    update_imu(state);
+    update_mag(state);
+
+    if (!state->imu_ok) {
+        return;
+    }
+
+    const float ax = state->accel_m_s2[0];
+    const float ay = state->accel_m_s2[1];
+    const float az = state->accel_m_s2[2];
+    const float accel_roll = atan2f(ay, az);
+    const float accel_pitch = atan2f(-ax, sqrtf(ay * ay + az * az));
+
+    // 简单互补滤波：真实部署第一步只需要稳定的 roll/pitch 保护。
+    const float alpha = 0.98f;
+    state->rpy_rad[0] = alpha * (state->rpy_rad[0] + state->gyro_rad_s[0] * dt_s) + (1.0f - alpha) * accel_roll;
+    state->rpy_rad[1] = alpha * (state->rpy_rad[1] + state->gyro_rad_s[1] * dt_s) + (1.0f - alpha) * accel_pitch;
+    state->rpy_rad[2] += state->gyro_rad_s[2] * dt_s;
+
+    if (state->mag_ok) {
+        const float heading = atan2f(state->mag_u_t[1], state->mag_u_t[0]);
+        state->rpy_rad[2] = 0.995f * state->rpy_rad[2] + 0.005f * heading;
+    }
+}

guguji_real_robot/firmware/esp32s3_espidf/main/sensors.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <stdbool.h>
+
+#include "esp_err.h"
+
+typedef struct {
+    float accel_m_s2[3];
+    float gyro_rad_s[3];
+    float mag_u_t[3];
+    float rpy_rad[3];
+    bool imu_ok;
+    bool mag_ok;
+} guguji_sensor_state_t;
+
+esp_err_t guguji_sensors_init(void);
+void guguji_sensors_update(guguji_sensor_state_t *state, float dt_s);

guguji_real_robot/firmware/esp32s3_espidf/main/status_outputs.c

@@ -0,0 +1,208 @@
+#include "status_outputs.h"
+
+#include <stdbool.h>
+
+#include "config.h"
+#include "driver/ledc.h"
+#include "driver/rmt.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+static const char *TAG = "status_outputs";
+
+#define BUZZER_LEDC_TIMER LEDC_TIMER_0
+#define BUZZER_LEDC_CHANNEL LEDC_CHANNEL_0
+#define BUZZER_LEDC_MODE LEDC_LOW_SPEED_MODE
+#define BUZZER_DUTY_RES LEDC_TIMER_10_BIT
+#define BUZZER_DUTY_ON 512
+
+static guguji_status_mode_t g_last_mode = GUGUJI_STATUS_BOOT;
+static uint32_t g_last_beep_ms = 0;
+static bool g_has_mode = false;
+
+static const char *status_name(guguji_status_mode_t mode)
+{
+    switch (mode) {
+    case GUGUJI_STATUS_BOOT:
+        return "boot";
+    case GUGUJI_STATUS_WAITING_HOST:
+        return "waiting_host";
+    case GUGUJI_STATUS_DISARMED:
+        return "disarmed";
+    case GUGUJI_STATUS_ARMED:
+        return "armed";
+    case GUGUJI_STATUS_TIMEOUT:
+        return "timeout";
+    case GUGUJI_STATUS_ESTOP:
+        return "estop";
+    case GUGUJI_STATUS_FAULT:
+        return "fault";
+    case GUGUJI_STATUS_SENSOR_WARN:
+        return "sensor_warn";
+    default:
+        return "unknown";
+    }
+}
+
+static void buzzer_silence(void)
+{
+    ledc_set_duty(BUZZER_LEDC_MODE, BUZZER_LEDC_CHANNEL, 0);
+    ledc_update_duty(BUZZER_LEDC_MODE, BUZZER_LEDC_CHANNEL);
+}
+
+static void buzzer_beep(uint32_t frequency_hz, uint32_t duration_ms)
+{
+    ledc_set_freq(BUZZER_LEDC_MODE, BUZZER_LEDC_TIMER, frequency_hz);
+    ledc_set_duty(BUZZER_LEDC_MODE, BUZZER_LEDC_CHANNEL, BUZZER_DUTY_ON);
+    ledc_update_duty(BUZZER_LEDC_MODE, BUZZER_LEDC_CHANNEL);
+    vTaskDelay(pdMS_TO_TICKS(duration_ms));
+    buzzer_silence();
+}
+
+static void buzzer_pattern_for_transition(guguji_status_mode_t mode)
+{
+    switch (mode) {
+    case GUGUJI_STATUS_ARMED:
+        buzzer_beep(2200, 70);
+        vTaskDelay(pdMS_TO_TICKS(45));
+        buzzer_beep(2600, 70);
+        break;
+    case GUGUJI_STATUS_DISARMED:
+        buzzer_beep(1200, 80);
+        break;
+    case GUGUJI_STATUS_TIMEOUT:
+        buzzer_beep(1500, 80);
+        vTaskDelay(pdMS_TO_TICKS(60));
+        buzzer_beep(1500, 80);
+        break;
+    case GUGUJI_STATUS_ESTOP:
+    case GUGUJI_STATUS_FAULT:
+        buzzer_beep(900, 260);
+        break;
+    case GUGUJI_STATUS_SENSOR_WARN:
+        buzzer_beep(1800, 60);
+        vTaskDelay(pdMS_TO_TICKS(40));
+        buzzer_beep(1000, 60);
+        break;
+    default:
+        break;
+    }
+}
+
+static void ws2812_write_rgb(uint8_t red, uint8_t green, uint8_t blue)
+{
+    rmt_item32_t items[24] = {0};
+    const uint8_t grb[3] = {green, red, blue};
+    int item_index = 0;
+
+    for (int byte_index = 0; byte_index < 3; ++byte_index) {
+        for (int bit = 7; bit >= 0; --bit) {
+            const bool one = ((grb[byte_index] >> bit) & 0x01u) != 0;
+            // RMT 时钟 40MHz，1 tick=25ns。下面约为 WS2812 的 0/1 码时序。
+            items[item_index].level0 = 1;
+            items[item_index].duration0 = one ? 34 : 14;
+            items[item_index].level1 = 0;
+            items[item_index].duration1 = one ? 14 : 34;
+            item_index++;
+        }
+    }
+
+    rmt_write_items(GUGUJI_WS2812_RMT_CHANNEL, items, 24, true);
+    rmt_wait_tx_done(GUGUJI_WS2812_RMT_CHANNEL, pdMS_TO_TICKS(20));
+    vTaskDelay(pdMS_TO_TICKS(1));
+}
+
+static void led_for_mode(guguji_status_mode_t mode, uint32_t now_ms)
+{
+    const bool slow_on = ((now_ms / 600) % 2) == 0;
+    const bool fast_on = ((now_ms / 180) % 2) == 0;
+    const bool double_flash = (now_ms % 1200) < 120 || ((now_ms + 220) % 1200) < 120;
+
+    switch (mode) {
+    case GUGUJI_STATUS_BOOT:
+        ws2812_write_rgb(24, 24, 24);
+        break;
+    case GUGUJI_STATUS_WAITING_HOST:
+        ws2812_write_rgb(0, 0, slow_on ? 40 : 6);
+        break;
+    case GUGUJI_STATUS_DISARMED:
+        ws2812_write_rgb(0, slow_on ? 26 : 4, slow_on ? 26 : 4);
+        break;
+    case GUGUJI_STATUS_ARMED:
+        ws2812_write_rgb(0, 42, 0);
+        break;
+    case GUGUJI_STATUS_TIMEOUT:
+        ws2812_write_rgb(double_flash ? 45 : 0, double_flash ? 24 : 0, 0);
+        break;
+    case GUGUJI_STATUS_ESTOP:
+    case GUGUJI_STATUS_FAULT:
+        ws2812_write_rgb(fast_on ? 55 : 0, 0, 0);
+        break;
+    case GUGUJI_STATUS_SENSOR_WARN:
+        ws2812_write_rgb(slow_on ? 28 : 3, 0, slow_on ? 42 : 6);
+        break;
+    default:
+        ws2812_write_rgb(0, 0, 0);
+        break;
+    }
+}
+
+esp_err_t guguji_status_outputs_init(void)
+{
+    ledc_timer_config_t timer_config = {
+        .speed_mode = BUZZER_LEDC_MODE,
+        .duty_resolution = BUZZER_DUTY_RES,
+        .timer_num = BUZZER_LEDC_TIMER,
+        .freq_hz = 2000,
+        .clk_cfg = LEDC_AUTO_CLK,
+    };
+    ESP_ERROR_CHECK(ledc_timer_config(&timer_config));
+
+    ledc_channel_config_t channel_config = {
+        .gpio_num = GUGUJI_BUZZER_GPIO,
+        .speed_mode = BUZZER_LEDC_MODE,
+        .channel = BUZZER_LEDC_CHANNEL,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = BUZZER_LEDC_TIMER,
+        .duty = 0,
+        .hpoint = 0,
+    };
+    ESP_ERROR_CHECK(ledc_channel_config(&channel_config));
+
+    rmt_config_t ws2812_rmt_config = RMT_DEFAULT_CONFIG_TX(GUGUJI_WS2812_GPIO, GUGUJI_WS2812_RMT_CHANNEL);
+    ws2812_rmt_config.clk_div = 2;
+    ws2812_rmt_config.mem_block_num = 1;
+    ESP_ERROR_CHECK(rmt_config(&ws2812_rmt_config));
+    ESP_ERROR_CHECK(rmt_driver_install(GUGUJI_WS2812_RMT_CHANNEL, 0, 0));
+
+    ws2812_write_rgb(0, 0, 0);
+    buzzer_beep(1600, 70);
+    vTaskDelay(pdMS_TO_TICKS(45));
+    buzzer_beep(2200, 70);
+    ESP_LOGI(TAG, "蜂鸣器 GPIO%d 与 WS2812 GPIO%d 初始化完成", GUGUJI_BUZZER_GPIO, GUGUJI_WS2812_GPIO);
+    return ESP_OK;
+}
+
+void guguji_status_outputs_apply(guguji_status_mode_t mode, uint32_t now_ms)
+{
+    if (!g_has_mode || mode != g_last_mode) {
+        ESP_LOGI(TAG, "状态提示切换: %s -> %s", g_has_mode ? status_name(g_last_mode) : "none", status_name(mode));
+        buzzer_pattern_for_transition(mode);
+        g_last_mode = mode;
+        g_has_mode = true;
+        g_last_beep_ms = now_ms;
+    }
+
+    if ((mode == GUGUJI_STATUS_FAULT || mode == GUGUJI_STATUS_ESTOP) && now_ms - g_last_beep_ms > 1500) {
+        buzzer_beep(900, 120);
+        g_last_beep_ms = now_ms;
+    } else if (mode == GUGUJI_STATUS_TIMEOUT && now_ms - g_last_beep_ms > 2500) {
+        buzzer_beep(1500, 70);
+        vTaskDelay(pdMS_TO_TICKS(50));
+        buzzer_beep(1500, 70);
+        g_last_beep_ms = now_ms;
+    }
+
+    led_for_mode(mode, now_ms);
+}

guguji_real_robot/firmware/esp32s3_espidf/main/status_outputs.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "esp_err.h"
+
+typedef enum {
+    GUGUJI_STATUS_BOOT = 0,
+    GUGUJI_STATUS_WAITING_HOST,
+    GUGUJI_STATUS_DISARMED,
+    GUGUJI_STATUS_ARMED,
+    GUGUJI_STATUS_TIMEOUT,
+    GUGUJI_STATUS_ESTOP,
+    GUGUJI_STATUS_FAULT,
+    GUGUJI_STATUS_SENSOR_WARN,
+} guguji_status_mode_t;
+
+esp_err_t guguji_status_outputs_init(void);
+void guguji_status_outputs_apply(guguji_status_mode_t mode, uint32_t now_ms);

guguji_real_robot/firmware/esp32s3_espidf/sdkconfig.defaults

@@ -0,0 +1,4 @@
+CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192
+CONFIG_FREERTOS_HZ=1000
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
+CONFIG_LWIP_SO_REUSE=y

guguji_real_robot/host/README.md

@@ -0,0 +1,18 @@
+# guguji real robot host bridge
+
+这个目录放主机端部署代码：本机加载训练好的 PPO 模型，通过 WiFi UDP 把 8 个关节目标角发送给 ESP32S3。
+
+最小运行命令：
+
+```bash
+cd /home/corvin/Project/guguji_simulation
+python3 -m venv .venv-real
+source .venv-real/bin/activate
+pip install -r guguji_rl/requirements.txt
+pip install -r guguji_real_robot/host/requirements.txt
+python guguji_real_robot/host/scripts/run_real_policy.py \
+  --model guguji_rl/outputs/<run>/final_model.zip \
+  --deterministic
+```
+
+确认机器人悬空、急停和电源都准备好后，再加 `--arm` 让 ESP32S3 使能电机。

guguji_real_robot/host/configs/guguji_real_robot.yaml

@@ -0,0 +1,28 @@
+network:
+  # 固件默认开 AP: SSID=guguji-robot，ESP32S3 地址通常是 192.168.4.1。
+  esp32_host: 192.168.4.1
+  command_port: 7777
+  telemetry_bind_host: 0.0.0.0
+  telemetry_port: 7778
+  telemetry_timeout_s: 1.0
+
+control:
+  # 第一版建议用偏软的增益做离地/悬空测试，再逐步加大。
+  kp: [25.0, 25.0, 18.0, 12.0, 25.0, 25.0, 18.0, 12.0]
+  kd: [0.45, 0.45, 0.35, 0.25, 0.45, 0.45, 0.35, 0.25]
+
+safety:
+  max_joint_step_rad: 0.035
+  max_roll_rad: 0.75
+  max_pitch_rad: 0.75
+
+state_estimation:
+  # 当前真实机没有外部定位/测高时，先用常量补齐训练观测里的高度和前进速度。
+  # 后续接入动捕、视觉里程计或轮廓测距后，可以在这里换成真实估计。
+  estimated_base_height_m: 0.35
+  estimated_forward_velocity_m_s: 0.0
+  target_forward_velocity_m_s: 0.18
+
+logging:
+  # 主机端周期性打印 ESP32S3 状态、故障分类、姿态和温度。
+  status_interval_s: 1.0

guguji_real_robot/host/guguji_real_bridge/__init__.py

@@ -0,0 +1,33 @@
+"""Host-side bridge for deploying guguji RL policies to the real robot."""
+
+from .action_mapper import DeployActionMapper
+from .protocol import (
+    COMMAND_FLAG_ARM,
+    COMMAND_FLAG_CLEAR_FAULTS,
+    COMMAND_FLAG_DISARM,
+    COMMAND_FLAG_ESTOP,
+    COMMAND_FLAG_HOLD,
+    ROBOT_STATE_NAMES,
+    TELEMETRY_FAULT_FEEDBACK_TIMEOUT_MASK,
+    TELEMETRY_FAULT_IMU_OFFLINE,
+    TELEMETRY_FAULT_MAG_OFFLINE,
+    TELEMETRY_FAULT_MOTOR_MASK,
+    CommandPacket,
+    TelemetryPacket,
+)
+
+__all__ = [
+    'COMMAND_FLAG_ARM',
+    'COMMAND_FLAG_CLEAR_FAULTS',
+    'COMMAND_FLAG_DISARM',
+    'COMMAND_FLAG_ESTOP',
+    'COMMAND_FLAG_HOLD',
+    'ROBOT_STATE_NAMES',
+    'TELEMETRY_FAULT_FEEDBACK_TIMEOUT_MASK',
+    'TELEMETRY_FAULT_IMU_OFFLINE',
+    'TELEMETRY_FAULT_MAG_OFFLINE',
+    'TELEMETRY_FAULT_MOTOR_MASK',
+    'CommandPacket',
+    'DeployActionMapper',
+    'TelemetryPacket',
+]

guguji_real_robot/host/guguji_real_bridge/action_mapper.py

@@ -0,0 +1,187 @@
+from __future__ import annotations
+
+import math
+import sys
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+HOST_ROOT = Path(__file__).resolve().parents[1]
+PROJECT_ROOT = HOST_ROOT.parents[1]
+RL_ROOT = PROJECT_ROOT / 'guguji_rl'
+if str(RL_ROOT) not in sys.path:
+    sys.path.insert(0, str(RL_ROOT))
+
+from guguji_rl.config import load_config, resolve_project_path
+from guguji_rl.urdf_utils import parse_joint_limits
+
+
+@dataclass(slots=True)
+class DeploymentObservation:
+    observation: np.ndarray
+    joint_targets: np.ndarray
+
+
+class DeployActionMapper:
+    """复用仿真环境里的动作映射逻辑，减少 sim-to-real 的隐藏差异。"""
+
+    def __init__(
+        self,
+        *,
+        rl_config_path: str | Path,
+        deploy_config: dict[str, Any],
+    ) -> None:
+        self.config = load_config(rl_config_path)
+        self.robot_config = self.config['robot']
+        self.sim_config = self.config['sim']
+        self.task_config = self.config['task']
+        self.reference_gait_config = self.robot_config.get('reference_gait', {})
+        self.reference_gait_enabled = bool(self.reference_gait_config.get('enabled', False))
+        self.reference_gait_period = max(
+            float(self.reference_gait_config.get('period', 0.9)),
+            float(self.sim_config['control_dt']),
+        )
+
+        urdf_path = resolve_project_path(self.config, self.robot_config['urdf_path'])
+        self.joint_limits = parse_joint_limits(urdf_path, self.robot_config['joint_names'])
+        self.joint_names = [joint_limit.name for joint_limit in self.joint_limits]
+        self.joint_lower = np.array([joint.lower for joint in self.joint_limits], dtype=np.float32)
+        self.joint_upper = np.array([joint.upper for joint in self.joint_limits], dtype=np.float32)
+        self.joint_mid = np.array([joint.midpoint for joint in self.joint_limits], dtype=np.float32)
+        self.joint_half_range = np.array([joint.half_range for joint in self.joint_limits], dtype=np.float32)
+
+        nominal_joint_config = self.robot_config.get('nominal_joint_positions', {})
+        self.nominal_joint_targets = np.array(
+            [float(nominal_joint_config.get(joint_name, 0.0)) for joint_name in self.joint_names],
+            dtype=np.float32,
+        )
+        self.action_scale = float(self.robot_config.get('action_scale', 1.0))
+        self.action_smoothing = float(np.clip(self.robot_config.get('action_smoothing', 0.0), 0.0, 0.99))
+        self.current_action_residual = np.zeros(len(self.joint_names), dtype=np.float32)
+        self.previous_action = np.zeros(len(self.joint_names), dtype=np.float32)
+        self.gait_phase = 0.0
+
+        self.estimated_base_height = float(deploy_config.get('estimated_base_height_m', 0.35))
+        self.estimated_forward_velocity = float(deploy_config.get('estimated_forward_velocity_m_s', 0.0))
+        self.target_forward_velocity = float(
+            deploy_config.get(
+                'target_forward_velocity_m_s',
+                self.task_config.get('target_forward_velocity', 0.0),
+            )
+        )
+
+    @property
+    def control_dt(self) -> float:
+        return float(self.sim_config['control_dt'])
+
+    def reset(self) -> None:
+        self.current_action_residual[:] = 0.0
+        self.previous_action[:] = 0.0
+        self.gait_phase = 0.0
+
+    def normalized_joint_position(self, joint_position: np.ndarray) -> np.ndarray:
+        return (joint_position - self.joint_mid) / self.joint_half_range
+
+    def build_observation(
+        self,
+        *,
+        joint_position: np.ndarray,
+        joint_velocity: np.ndarray,
+        roll: float,
+        pitch: float,
+    ) -> np.ndarray:
+        joint_position = np.asarray(joint_position, dtype=np.float32)
+        joint_velocity = np.asarray(joint_velocity, dtype=np.float32)
+        return np.concatenate(
+            [
+                self.normalized_joint_position(joint_position),
+                joint_velocity,
+                self.previous_action,
+                np.array(
+                    [
+                        self.estimated_base_height,
+                        roll,
+                        pitch,
+                        self.estimated_forward_velocity,
+                        self.target_forward_velocity,
+                    ],
+                    dtype=np.float32,
+                ),
+            ],
+            dtype=np.float32,
+        )
+
+    def action_to_joint_targets(self, action: np.ndarray) -> np.ndarray:
+        action = np.asarray(action, dtype=np.float32)
+        clipped_action = np.clip(action, -1.0, 1.0)
+        reference_targets = self.nominal_joint_targets + self.reference_gait_offsets()
+        desired_residual = clipped_action * self.joint_half_range * self.action_scale
+
+        if self.action_smoothing > 0.0:
+            smoothed_residual = (
+                self.action_smoothing * self.current_action_residual
+                + (1.0 - self.action_smoothing) * desired_residual
+            )
+        else:
+            smoothed_residual = desired_residual
+
+        self.current_action_residual = smoothed_residual.astype(np.float32)
+        joint_targets = reference_targets + self.current_action_residual
+        self.previous_action = clipped_action.copy()
+        self.advance_gait_phase()
+        return np.clip(joint_targets, self.joint_lower, self.joint_upper).astype(np.float32)
+
+    def reference_gait_offsets(self) -> np.ndarray:
+        offsets = np.zeros(len(self.joint_names), dtype=np.float32)
+        if not self.reference_gait_enabled:
+            return offsets
+
+        hip_amplitude = float(self.reference_gait_config.get('hip_pitch_amplitude', 0.0))
+        hip_bias = float(self.reference_gait_config.get('hip_pitch_bias', 0.0))
+        knee_amplitude = float(self.reference_gait_config.get('knee_pitch_amplitude', 0.0))
+        knee_bias = float(self.reference_gait_config.get('knee_pitch_bias', 0.0))
+        swing_knee_scale = float(self.reference_gait_config.get('swing_knee_scale', 1.0))
+        ankle_amplitude = float(self.reference_gait_config.get('ankle_pitch_amplitude', 0.0))
+        ankle_bias = float(self.reference_gait_config.get('ankle_pitch_bias', 0.0))
+        push_off_ankle_scale = float(self.reference_gait_config.get('push_off_ankle_scale', 0.0))
+        stance_ratio = float(np.clip(self.reference_gait_config.get('stance_ratio', 0.62), 0.05, 0.95))
+
+        def gait_profile(phase: float) -> tuple[float, float, float]:
+            phase = phase % 1.0
+            if phase < stance_ratio:
+                stance_progress = phase / stance_ratio
+                hip_profile = 1.0 - 2.0 * stance_progress
+                knee_profile = 0.18 * math.sin(math.pi * stance_progress)
+                push_off_progress = max((stance_progress - 0.60) / 0.40, 0.0)
+                ankle_profile = -0.70 * hip_profile + push_off_ankle_scale * push_off_progress
+            else:
+                swing_progress = (phase - stance_ratio) / (1.0 - stance_ratio)
+                hip_profile = -1.0 + 2.0 * swing_progress
+                knee_profile = swing_knee_scale * math.sin(math.pi * swing_progress)
+                ankle_profile = -0.45 * hip_profile - 0.25 * math.sin(math.pi * swing_progress)
+            return hip_profile, knee_profile, ankle_profile
+
+        left_hip, left_knee, left_ankle = gait_profile(self.gait_phase)
+        right_hip, right_knee, right_ankle = gait_profile(self.gait_phase + 0.5)
+
+        for index, joint_name in enumerate(self.joint_names):
+            if joint_name == 'left_hip_pitch_joint':
+                offsets[index] = hip_bias + hip_amplitude * left_hip
+            elif joint_name == 'right_hip_pitch_joint':
+                offsets[index] = hip_bias + hip_amplitude * right_hip
+            elif joint_name == 'left_knee_pitch_joint':
+                offsets[index] = knee_bias + knee_amplitude * left_knee
+            elif joint_name == 'right_knee_pitch_joint':
+                offsets[index] = knee_bias + knee_amplitude * right_knee
+            elif joint_name == 'left_ankle_pitch_joint':
+                offsets[index] = ankle_bias + ankle_amplitude * left_ankle
+            elif joint_name == 'right_ankle_pitch_joint':
+                offsets[index] = ankle_bias + ankle_amplitude * right_ankle
+
+        return offsets
+
+    def advance_gait_phase(self) -> None:
+        if self.reference_gait_enabled:
+            self.gait_phase = (self.gait_phase + self.control_dt / self.reference_gait_period) % 1.0

guguji_real_robot/host/guguji_real_bridge/config.py

@@ -0,0 +1,16 @@
+from __future__ import annotations
+
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+def load_yaml(path: str | Path) -> dict[str, Any]:
+    path = Path(path).resolve()
+    with path.open('r', encoding='utf-8') as file:
+        data = yaml.safe_load(file) or {}
+    if not isinstance(data, dict):
+        raise ValueError(f'配置文件必须是 YAML 字典: {path}')
+    data['meta'] = {'config_path': str(path)}
+    return data

guguji_real_robot/host/guguji_real_bridge/protocol.py

@@ -0,0 +1,246 @@
+from __future__ import annotations
+
+import binascii
+import socket
+import struct
+import time
+from dataclasses import dataclass
+from typing import Iterable
+
+import numpy as np
+
+
+JOINT_COUNT = 8
+PROTOCOL_VERSION = 1
+
+COMMAND_MAGIC = int.from_bytes(b'GUGC', byteorder='little')
+TELEMETRY_MAGIC = int.from_bytes(b'GUGT', byteorder='little')
+
+COMMAND_TYPE_SETPOINTS = 1
+
+COMMAND_FLAG_ARM = 1 << 0
+COMMAND_FLAG_DISARM = 1 << 1
+COMMAND_FLAG_HOLD = 1 << 2
+COMMAND_FLAG_ESTOP = 1 << 3
+COMMAND_FLAG_CLEAR_FAULTS = 1 << 4
+
+ROBOT_STATE_DISARMED = 0
+ROBOT_STATE_ARMED = 1
+ROBOT_STATE_ESTOP = 2
+ROBOT_STATE_TIMEOUT = 3
+ROBOT_STATE_NAMES = {
+    ROBOT_STATE_DISARMED: 'DISARMED',
+    ROBOT_STATE_ARMED: 'ARMED',
+    ROBOT_STATE_ESTOP: 'ESTOP',
+    ROBOT_STATE_TIMEOUT: 'TIMEOUT',
+}
+
+TELEMETRY_FAULT_MOTOR_MASK = 0x000000FF
+TELEMETRY_FAULT_FEEDBACK_TIMEOUT_MASK = 0x0000FF00
+TELEMETRY_FAULT_IMU_OFFLINE = 1 << 16
+TELEMETRY_FAULT_MAG_OFFLINE = 1 << 17
+
+COMMAND_BODY_FORMAT = '<IBBHIII'
+COMMAND_BODY_SIZE = struct.calcsize(COMMAND_BODY_FORMAT) + 5 * JOINT_COUNT * 4
+COMMAND_PACKET_SIZE = COMMAND_BODY_SIZE + 4
+
+TELEMETRY_BODY_FORMAT = '<IBBHIIII'
+TELEMETRY_BODY_SIZE = struct.calcsize(TELEMETRY_BODY_FORMAT) + 44 * 4
+TELEMETRY_PACKET_SIZE = TELEMETRY_BODY_SIZE + 4
+
+
+def _as_float_array(values: Iterable[float], count: int, *, name: str) -> np.ndarray:
+    array = np.asarray(list(values), dtype=np.float32)
+    if array.shape != (count,):
+        raise ValueError(f'{name} 需要 {count} 个 float，实际得到 {array.shape}')
+    return array
+
+
+def _crc32(payload: bytes) -> int:
+    return binascii.crc32(payload) & 0xFFFFFFFF
+
+
+def _milliseconds_now() -> int:
+    return int(time.monotonic() * 1000.0) & 0xFFFFFFFF
+
+
+@dataclass(slots=True)
+class CommandPacket:
+    sequence: int
+    positions_rad: np.ndarray
+    velocities_rad_s: np.ndarray
+    kp: np.ndarray
+    kd: np.ndarray
+    torque_ff_nm: np.ndarray
+    flags: int = 0
+    monotonic_ms: int | None = None
+
+    def pack(self) -> bytes:
+        positions = _as_float_array(self.positions_rad, JOINT_COUNT, name='positions_rad')
+        velocities = _as_float_array(self.velocities_rad_s, JOINT_COUNT, name='velocities_rad_s')
+        kp = _as_float_array(self.kp, JOINT_COUNT, name='kp')
+        kd = _as_float_array(self.kd, JOINT_COUNT, name='kd')
+        torque = _as_float_array(self.torque_ff_nm, JOINT_COUNT, name='torque_ff_nm')
+        body = struct.pack(
+            COMMAND_BODY_FORMAT,
+            COMMAND_MAGIC,
+            PROTOCOL_VERSION,
+            COMMAND_TYPE_SETPOINTS,
+            JOINT_COUNT,
+            int(self.sequence) & 0xFFFFFFFF,
+            _milliseconds_now() if self.monotonic_ms is None else int(self.monotonic_ms) & 0xFFFFFFFF,
+            int(self.flags) & 0xFFFFFFFF,
+        )
+        body += struct.pack(
+            '<' + 'f' * 5 * JOINT_COUNT,
+            *positions.tolist(),
+            *velocities.tolist(),
+            *kp.tolist(),
+            *kd.tolist(),
+            *torque.tolist(),
+        )
+        return body + struct.pack('<I', _crc32(body))
+
+
+@dataclass(slots=True)
+class TelemetryPacket:
+    robot_state: int
+    sequence: int
+    uptime_ms: int
+    last_command_age_ms: int
+    fault_mask: int
+    joint_position_rad: np.ndarray
+    joint_velocity_rad_s: np.ndarray
+    joint_torque_nm: np.ndarray
+    motor_temperature_c: np.ndarray
+    accel_m_s2: np.ndarray
+    gyro_rad_s: np.ndarray
+    mag_u_t: np.ndarray
+    rpy_rad: np.ndarray
+
+    @property
+    def roll(self) -> float:
+        return float(self.rpy_rad[0])
+
+    @property
+    def pitch(self) -> float:
+        return float(self.rpy_rad[1])
+
+    @property
+    def yaw(self) -> float:
+        return float(self.rpy_rad[2])
+
+    @property
+    def robot_state_name(self) -> str:
+        return ROBOT_STATE_NAMES.get(self.robot_state, f'UNKNOWN({self.robot_state})')
+
+    @property
+    def motor_fault_mask(self) -> int:
+        return self.fault_mask & TELEMETRY_FAULT_MOTOR_MASK
+
+    @property
+    def feedback_timeout_mask(self) -> int:
+        return (self.fault_mask & TELEMETRY_FAULT_FEEDBACK_TIMEOUT_MASK) >> 8
+
+    @property
+    def imu_offline(self) -> bool:
+        return (self.fault_mask & TELEMETRY_FAULT_IMU_OFFLINE) != 0
+
+    @property
+    def mag_offline(self) -> bool:
+        return (self.fault_mask & TELEMETRY_FAULT_MAG_OFFLINE) != 0
+
+    @property
+    def max_motor_temperature_c(self) -> float:
+        if self.motor_temperature_c.size == 0:
+            return 0.0
+        return float(np.max(self.motor_temperature_c))
+
+
+def unpack_telemetry(packet: bytes) -> TelemetryPacket:
+    if len(packet) != TELEMETRY_PACKET_SIZE:
+        raise ValueError(f'遥测包长度错误: {len(packet)} != {TELEMETRY_PACKET_SIZE}')
+    body = packet[:-4]
+    received_crc, = struct.unpack('<I', packet[-4:])
+    if _crc32(body) != received_crc:
+        raise ValueError('遥测包 CRC 校验失败')
+
+    header_size = struct.calcsize(TELEMETRY_BODY_FORMAT)
+    (
+        magic,
+        version,
+        robot_state,
+        joint_count,
+        sequence,
+        uptime_ms,
+        last_command_age_ms,
+        fault_mask,
+    ) = struct.unpack(TELEMETRY_BODY_FORMAT, body[:header_size])
+    if magic != TELEMETRY_MAGIC:
+        raise ValueError('遥测包 magic 不匹配')
+    if version != PROTOCOL_VERSION:
+        raise ValueError(f'遥测协议版本不匹配: {version}')
+    if joint_count != JOINT_COUNT:
+        raise ValueError(f'遥测关节数量不匹配: {joint_count}')
+
+    floats = np.array(
+        struct.unpack('<' + 'f' * 44, body[header_size:]),
+        dtype=np.float32,
+    )
+    offset = 0
+
+    def take(count: int) -> np.ndarray:
+        nonlocal offset
+        result = floats[offset:offset + count].copy()
+        offset += count
+        return result
+
+    return TelemetryPacket(
+        robot_state=robot_state,
+        sequence=sequence,
+        uptime_ms=uptime_ms,
+        last_command_age_ms=last_command_age_ms,
+        fault_mask=fault_mask,
+        joint_position_rad=take(JOINT_COUNT),
+        joint_velocity_rad_s=take(JOINT_COUNT),
+        joint_torque_nm=take(JOINT_COUNT),
+        motor_temperature_c=take(JOINT_COUNT),
+        accel_m_s2=take(3),
+        gyro_rad_s=take(3),
+        mag_u_t=take(3),
+        rpy_rad=take(3),
+    )
+
+
+class UdpRobotLink:
+    """主机和 ESP32S3 固件之间的 UDP 通道。"""
+
+    def __init__(
+        self,
+        *,
+        command_host: str,
+        command_port: int,
+        telemetry_bind_host: str,
+        telemetry_port: int,
+        timeout_s: float,
+    ) -> None:
+        self._command_address = (command_host, int(command_port))
+        self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self._socket.bind((telemetry_bind_host, int(telemetry_port)))
+        self._socket.settimeout(float(timeout_s))
+
+    def send_command(self, command: CommandPacket) -> None:
+        self._socket.sendto(command.pack(), self._command_address)
+
+    def receive_telemetry(self) -> TelemetryPacket:
+        while True:
+            data, _ = self._socket.recvfrom(2048)
+            try:
+                return unpack_telemetry(data)
+            except ValueError:
+                # UDP 可能收到旧包、截断包或非本协议包，部署循环里直接跳过。
+                continue
+
+    def close(self) -> None:
+        self._socket.close()

guguji_real_robot/host/guguji_real_bridge/safety.py

@@ -0,0 +1,26 @@
+from __future__ import annotations
+
+import numpy as np
+
+
+class TargetLimiter:
+    """部署侧限幅器，避免策略输出在真实机上突然跳变。"""
+
+    def __init__(self, *, max_step_rad: float, lower: np.ndarray, upper: np.ndarray) -> None:
+        self.max_step_rad = float(max_step_rad)
+        self.lower = lower.astype(np.float32)
+        self.upper = upper.astype(np.float32)
+        self._previous: np.ndarray | None = None
+
+    def reset(self, initial: np.ndarray) -> None:
+        self._previous = np.asarray(initial, dtype=np.float32).copy()
+
+    def apply(self, target: np.ndarray) -> np.ndarray:
+        target = np.asarray(target, dtype=np.float32)
+        if self._previous is None:
+            self.reset(target)
+            return np.clip(target, self.lower, self.upper)
+        delta = np.clip(target - self._previous, -self.max_step_rad, self.max_step_rad)
+        limited = np.clip(self._previous + delta, self.lower, self.upper)
+        self._previous = limited.copy()
+        return limited

guguji_real_robot/host/requirements.txt

@@ -0,0 +1,4 @@
+numpy
+pyyaml
+stable-baselines3
+torch

guguji_real_robot/host/scripts/run_real_policy.py

@@ -0,0 +1,291 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import argparse
+import sys
+import time
+from pathlib import Path
+
+import numpy as np
+
+HOST_ROOT = Path(__file__).resolve().parents[1]
+PROJECT_ROOT = HOST_ROOT.parents[1]
+if str(HOST_ROOT) not in sys.path:
+    sys.path.insert(0, str(HOST_ROOT))
+if str(PROJECT_ROOT / 'guguji_rl') not in sys.path:
+    sys.path.insert(0, str(PROJECT_ROOT / 'guguji_rl'))
+
+from guguji_real_bridge.action_mapper import DeployActionMapper
+from guguji_real_bridge.config import load_yaml
+from guguji_real_bridge.protocol import (
+    COMMAND_FLAG_ARM,
+    COMMAND_FLAG_CLEAR_FAULTS,
+    COMMAND_FLAG_DISARM,
+    COMMAND_FLAG_ESTOP,
+    COMMAND_FLAG_HOLD,
+    TELEMETRY_FAULT_IMU_OFFLINE,
+    CommandPacket,
+    TelemetryPacket,
+    UdpRobotLink,
+)
+from guguji_real_bridge.safety import TargetLimiter
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description='Run a trained PPO policy on the real guguji robot through WiFi.')
+    parser.add_argument('--deploy-config', default=str(HOST_ROOT / 'configs' / 'guguji_real_robot.yaml'))
+    parser.add_argument('--rl-config', default=str(PROJECT_ROOT / 'guguji_rl' / 'configs' / 'walk_ppo.yaml'))
+    parser.add_argument('--model', required=True, help='训练好的 stable-baselines3 PPO 模型 .zip 路径')
+    parser.add_argument('--arm', action='store_true', help='真正使能并下发运动指令；不加时只保持失能')
+    parser.add_argument('--clear-faults', action='store_true', help='启动时请求 ESP32S3 清除 RS00 故障')
+    parser.add_argument('--max-seconds', type=float, default=0.0, help='运行时长，0 表示一直运行')
+    parser.add_argument('--deterministic', action='store_true', help='使用确定性策略输出')
+    parser.add_argument('--log-interval', type=float, default=None, help='ESP32S3 遥测日志间隔，单位秒')
+    return parser.parse_args()
+
+
+def load_model(model_path: str | Path):
+    try:
+        from stable_baselines3 import PPO
+    except ImportError as exc:
+        raise RuntimeError('缺少 stable-baselines3，请先安装 guguji_real_robot/host/requirements.txt') from exc
+    return PPO.load(str(Path(model_path).resolve()))
+
+
+def _mask_names(mask: int, names: list[str]) -> list[str]:
+    return [name for index, name in enumerate(names) if (mask & (1 << index)) != 0]
+
+
+def format_fault_summary(telemetry: TelemetryPacket, joint_names: list[str]) -> str:
+    parts: list[str] = []
+    motor_faults = _mask_names(telemetry.motor_fault_mask, joint_names)
+    feedback_timeouts = _mask_names(telemetry.feedback_timeout_mask, joint_names)
+    if motor_faults:
+        parts.append('motor_fault=' + ','.join(motor_faults))
+    if feedback_timeouts:
+        parts.append('feedback_timeout=' + ','.join(feedback_timeouts))
+    if telemetry.imu_offline:
+        parts.append('imu_offline')
+    if telemetry.mag_offline:
+        parts.append('mag_offline')
+    return '; '.join(parts) if parts else 'ok'
+
+
+def print_telemetry_status(telemetry: TelemetryPacket, joint_names: list[str]) -> None:
+    rpy_deg = np.degrees(telemetry.rpy_rad)
+    mean_abs_velocity = float(np.mean(np.abs(telemetry.joint_velocity_rad_s)))
+    print(
+        '[ESP32S3] '
+        f'state={telemetry.robot_state_name} '
+        f'seq={telemetry.sequence} '
+        f'uptime={telemetry.uptime_ms / 1000.0:.1f}s '
+        f'cmd_age={telemetry.last_command_age_ms}ms '
+        f'faults={format_fault_summary(telemetry, joint_names)} '
+        f'rpy=({rpy_deg[0]:.1f},{rpy_deg[1]:.1f},{rpy_deg[2]:.1f})deg '
+        f'temp_max={telemetry.max_motor_temperature_c:.1f}C '
+        f'joint_vel_mean={mean_abs_velocity:.3f}rad/s'
+    )
+
+
+def main() -> int:
+    args = parse_args()
+    deploy_config = load_yaml(args.deploy_config)
+    network_config = deploy_config['network']
+    control_config = deploy_config['control']
+    safety_config = deploy_config['safety']
+    logging_config = deploy_config.get('logging', {})
+    log_interval_s = args.log_interval
+    if log_interval_s is None:
+        log_interval_s = float(logging_config.get('status_interval_s', 1.0))
+
+    mapper = DeployActionMapper(rl_config_path=args.rl_config, deploy_config=deploy_config['state_estimation'])
+    limiter = TargetLimiter(
+        max_step_rad=float(safety_config['max_joint_step_rad']),
+        lower=mapper.joint_lower,
+        upper=mapper.joint_upper,
+    )
+    model = load_model(args.model)
+    link = UdpRobotLink(
+        command_host=str(network_config['esp32_host']),
+        command_port=int(network_config['command_port']),
+        telemetry_bind_host=str(network_config.get('telemetry_bind_host', '0.0.0.0')),
+        telemetry_port=int(network_config['telemetry_port']),
+        timeout_s=float(network_config.get('telemetry_timeout_s', 1.0)),
+    )
+
+    kp = np.asarray(control_config['kp'], dtype=np.float32)
+    kd = np.asarray(control_config['kd'], dtype=np.float32)
+    zero_velocity = np.zeros(len(mapper.joint_names), dtype=np.float32)
+    zero_torque = np.zeros(len(mapper.joint_names), dtype=np.float32)
+    sequence = 0
+    start_time = time.monotonic()
+    next_tick = start_time
+    first_telemetry = True
+    last_log_time = 0.0
+    last_state: int | None = None
+    last_fault_mask: int | None = None
+    last_safety_message: str | None = None
+
+    print(
+        '主机部署配置: '
+        f'esp32={network_config["esp32_host"]}:{int(network_config["command_port"])} '
+        f'telemetry_port={int(network_config["telemetry_port"])} '
+        f'rl_config={Path(args.rl_config).resolve()} '
+        f'model={Path(args.model).resolve()} '
+        f'arm={args.arm}'
+    )
+
+    # 固件只有收到第一条 UDP 命令后，才知道遥测应该回传到哪个主机和端口。
+    # 所以这里先发几帧“失能 + 保持”的安全包，建立回传路径。
+    for _ in range(5):
+        link.send_command(
+            CommandPacket(
+                sequence=sequence,
+                positions_rad=mapper.nominal_joint_targets.astype(np.float32),
+                velocities_rad_s=zero_velocity,
+                kp=kp,
+                kd=kd,
+                torque_ff_nm=zero_torque,
+                flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD,
+            )
+        )
+        sequence = (sequence + 1) & 0xFFFFFFFF
+        time.sleep(0.05)
+
+    print('等待 ESP32S3 遥测...')
+    try:
+        while True:
+            try:
+                telemetry = link.receive_telemetry()
+            except TimeoutError:
+                link.send_command(
+                    CommandPacket(
+                        sequence=sequence,
+                        positions_rad=mapper.nominal_joint_targets.astype(np.float32),
+                        velocities_rad_s=zero_velocity,
+                        kp=kp,
+                        kd=kd,
+                        torque_ff_nm=zero_torque,
+                        flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD | COMMAND_FLAG_ESTOP,
+                    )
+                )
+                sequence = (sequence + 1) & 0xFFFFFFFF
+                print('等待遥测超时，已发送失能安全包，继续等待...')
+                continue
+
+            if first_telemetry:
+                limiter.reset(telemetry.joint_position_rad)
+                mapper.reset()
+                first_telemetry = False
+                start_time = time.monotonic()
+                next_tick = start_time
+                print('已收到遥测，开始部署循环。')
+                print_telemetry_status(telemetry, mapper.joint_names)
+                last_log_time = time.monotonic()
+                last_state = telemetry.robot_state
+                last_fault_mask = telemetry.fault_mask
+
+            elapsed = time.monotonic() - start_time
+            if args.max_seconds > 0 and elapsed >= args.max_seconds:
+                break
+
+            now = time.monotonic()
+            should_log = (
+                telemetry.robot_state != last_state
+                or telemetry.fault_mask != last_fault_mask
+                or (log_interval_s > 0 and now - last_log_time >= log_interval_s)
+            )
+            if should_log:
+                print_telemetry_status(telemetry, mapper.joint_names)
+                last_log_time = now
+                last_state = telemetry.robot_state
+                last_fault_mask = telemetry.fault_mask
+
+            observation = mapper.build_observation(
+                joint_position=telemetry.joint_position_rad,
+                joint_velocity=telemetry.joint_velocity_rad_s,
+                roll=telemetry.roll,
+                pitch=telemetry.pitch,
+            )
+            action, _ = model.predict(observation, deterministic=args.deterministic)
+            target = mapper.action_to_joint_targets(action)
+            target = limiter.apply(target)
+
+            flags = 0
+            if args.clear_faults and sequence < 10:
+                flags |= COMMAND_FLAG_CLEAR_FAULTS
+            if args.arm:
+                flags |= COMMAND_FLAG_ARM
+            else:
+                flags |= COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD
+
+            safety_messages: list[str] = []
+            if telemetry.motor_fault_mask != 0:
+                flags |= COMMAND_FLAG_ESTOP
+                safety_messages.append(f'检测到电机故障: {format_fault_summary(telemetry, mapper.joint_names)}')
+
+            if args.arm and telemetry.feedback_timeout_mask != 0:
+                flags |= COMMAND_FLAG_ESTOP
+                safety_messages.append(f'电机反馈超时: {format_fault_summary(telemetry, mapper.joint_names)}')
+
+            if args.arm and (telemetry.fault_mask & TELEMETRY_FAULT_IMU_OFFLINE) != 0:
+                flags |= COMMAND_FLAG_ESTOP
+                safety_messages.append('QMI8658A IMU 离线，roll/pitch 安全保护不可用')
+
+            if abs(telemetry.roll) > float(safety_config['max_roll_rad']):
+                flags |= COMMAND_FLAG_ESTOP
+                safety_messages.append(f'roll 超限 {telemetry.roll:.3f} rad')
+            if abs(telemetry.pitch) > float(safety_config['max_pitch_rad']):
+                flags |= COMMAND_FLAG_ESTOP
+                safety_messages.append(f'pitch 超限 {telemetry.pitch:.3f} rad')
+
+            safety_message = '；'.join(safety_messages)
+            if safety_message and safety_message != last_safety_message:
+                print(f'{safety_message}，已请求急停。')
+                last_safety_message = safety_message
+            elif not safety_message:
+                last_safety_message = None
+
+            link.send_command(
+                CommandPacket(
+                    sequence=sequence,
+                    positions_rad=target,
+                    velocities_rad_s=zero_velocity,
+                    kp=kp,
+                    kd=kd,
+                    torque_ff_nm=zero_torque,
+                    flags=flags,
+                )
+            )
+            sequence = (sequence + 1) & 0xFFFFFFFF
+
+            next_tick += mapper.control_dt
+            sleep_s = next_tick - time.monotonic()
+            if sleep_s > 0.0:
+                time.sleep(sleep_s)
+
+    except KeyboardInterrupt:
+        print('收到 Ctrl+C，发送失能保持命令后退出。')
+    finally:
+        hold = mapper.nominal_joint_targets.astype(np.float32)
+        for _ in range(5):
+            link.send_command(
+                CommandPacket(
+                    sequence=sequence,
+                    positions_rad=hold,
+                    velocities_rad_s=zero_velocity,
+                    kp=kp,
+                    kd=kd,
+                    torque_ff_nm=zero_torque,
+                    flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD,
+                )
+            )
+            sequence = (sequence + 1) & 0xFFFFFFFF
+            time.sleep(0.02)
+        link.close()
+
+    return 0
+
+
+if __name__ == '__main__':
+    raise SystemExit(main())