可以正常控制关节电机，现在需要调试电机限位

guguji_real_robot/docs/real_robot_deployment_guide.md

@@ -24,13 +24,15 @@
 
 当前默认引脚：
 
-- ESP32S3 `GPIO5` -> TJA1050T TXD
-- ESP32S3 `GPIO4` -> TJA1050T RXD
+- ESP32S3 `GPIO41` -> TJA1050T TXD
+- ESP32S3 `GPIO40` -> TJA1050T RXD
 - TJA1050T CANH/CANL -> RS00 CANH/CANL 总线
 - 总线两端各 120Ω 终端电阻
 - ESP32S3 GND、RS00 电源 GND 共地
 - RS00 电机按手册供电，额定 48V，允许 24V-60V
 
+断电后测量 CANH/CANL 之间电阻：约 `60Ω` 表示两端各有 120Ω 终端，约 `120Ω` 表示只有一端终端。短线单电机调试可能可以先通信，但整机线束建议在电机总线末端补一个 120Ω。
+
 无屏状态提示硬件：
 
 - `GPIO42` -> 无源蜂鸣器
@@ -149,10 +151,13 @@ faults=feedback_timeout=left_knee_pitch_joint; imu_offline
 
 ```bash
 python guguji_real_robot/host/scripts/run_real_policy.py \
+  --deploy-config guguji_real_robot/host/configs/guguji_real_robot_bench.yaml \
   --model guguji_rl/outputs/<你的训练目录>/final_model.zip \
   --deterministic \
   --clear-faults \
-  --arm
+  --arm \
+  --confirm-calibrated \
+  --max-seconds 3
 ```
 
 可用 `--log-interval 0.5` 临时提高日志刷新率。默认值在：
@@ -165,13 +170,20 @@ guguji_real_robot/host/configs/guguji_real_robot.yaml
 
 第一次不要让机器人落地，务必悬空测试。
 
+详细流程见：
+
+```text
+guguji_real_robot/docs/real_robot_joint_calibration_guide.md
+```
+
 标定步骤：
 
-1. 使用官方工具或本固件的零偏配置，让每个关节机械零位和仿真名义站姿对应。
-2. 单独给每个关节一个很小的正向目标角，观察真实转动方向。
-3. 如果方向相反，在 `robot_config.h` 中把该关节 `direction` 从 `1.0f` 改成 `-1.0f`。
-4. 如果零位有偏差，修改 `zero_offset_rad`。
-5. 每次修改后重新编译刷写。
+1. 先运行 `host/scripts/read_robot_state.py`，在不使能电机的情况下读取每个关节 `pos_rad`。
+2. 修改 `robot_config.h` 的 `zero_offset_rad`，让机械中立姿态映射到仿真名义角度。
+3. 重新刷写固件，再次只读遥测，确认 8 个关节都在配置限位内。
+4. 使用 `host/scripts/joint_jog_test.py` 单独给每个关节一个很小的正向目标角，观察真实转动方向。
+5. 如果方向相反，在 `robot_config.h` 中把该关节 `direction` 从 `1.0f` 改成 `-1.0f`。
+6. 每次修改后重新编译刷写。
 
 映射关系是：
 
@@ -191,6 +203,8 @@ guguji_real_robot/host/configs/guguji_real_robot.yaml
 
 - `max_joint_step_rad`：每个控制周期允许目标角变化的最大值。
 - `max_roll_rad` / `max_pitch_rad`：超过后主机请求急停。
+- `max_joint_velocity_rad_s`：关节速度超过后主机立即急停并退出。
+- `arm_position_limit_margin_rad`：首帧遥测超出限位太多时，主机拒绝使能。
 - `kp` / `kd`：RS00 MIT 运控模式的关节增益。
 
 固件侧安全参数在 `config.h`：
@@ -198,6 +212,8 @@ guguji_real_robot/host/configs/guguji_real_robot.yaml
 - `GUGUJI_COMMAND_TIMEOUT_MS`：超过这个时间没有收到主机命令就停止电机。
 - `GUGUJI_CONTROL_PERIOD_MS`：CAN 控制周期，默认 20ms，与当前训练 `control_dt=0.05s` 相比更快，主机会按训练周期发新目标，固件保持最近目标。
 - `GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS`：超过这个时间没有收到某个 RS00 的反馈，就在遥测中标记该关节反馈超时。
+- `GUGUJI_ARM_POSITION_LIMIT_MARGIN_RAD`：使能前当前反馈必须在 `robot_config.h` 限位附近。
+- `GUGUJI_MAX_COMMAND_POSITION_ERROR_RAD`：单帧目标和当前反馈差太大时，固件拒绝控制并进入 `ESTOP`。
 
 ## 9. 当前限制
 

guguji_real_robot/docs/real_robot_joint_calibration_guide.md

@@ -0,0 +1,117 @@
+# 真机关节零位和方向标定流程
+
+这一步必须在任何整机策略 `--arm` 前完成。通信 `faults=ok` 只说明链路正常，不代表
+电机机械零位、关节方向和限位已经和仿真一致。
+
+## 1. 先只读遥测
+
+机器人断开负载风险，修复所有机械结构后，让每个关节处在你希望的机械中立姿态。
+不要添加 `--arm`。
+
+```bash
+cd /home/corvin/Project/guguji_simulation
+
+python guguji_real_robot/host/scripts/read_robot_state.py \
+  --deploy-config guguji_real_robot/host/configs/guguji_real_robot_bench.yaml \
+  --samples 3
+```
+
+重点看每个关节的 `pos_rad`。如果机械中立姿态下某个关节希望是 `0rad`，
+但读数明显不是 0，就必须先改 `robot_config.h` 里的 `zero_offset_rad`。
+
+## 2. 计算 zero_offset_rad
+
+固件映射关系：
+
+```text
+motor_angle = joint_angle * direction + zero_offset_rad
+joint_angle = (motor_angle - zero_offset_rad) * direction
+```
+
+如果当前配置为：
+
+```text
+direction_old
+zero_offset_old
+```
+
+只读遥测得到当前关节角：
+
+```text
+q_read
+```
+
+希望当前机械姿态对应仿真角：
+
+```text
+q_desired
+```
+
+新的零偏建议先按下面公式计算：
+
+```text
+zero_offset_new = zero_offset_old + q_read * direction_old - q_desired * direction_new
+```
+
+首轮通常不改方向，且机械中立姿态希望是 `0rad`，此时公式简化为：
+
+```text
+zero_offset_new = zero_offset_old + q_read * direction_old
+```
+
+示例：4 号关节 `left_ankle_joint` 当前 `direction=1.0f`、`zero_offset=0.0f`，
+机械中立姿态下只读遥测 `pos_rad=2.400`，希望中立姿态是 `0rad`，则先改成：
+
+```c
+{"left_ankle_joint", 4, 1.0f, 2.400f, -0.6f, 0.6f},
+```
+
+修改后重新编译、刷写固件，再运行只读遥测。机械中立姿态下，该关节应接近 `0rad`。
+
+## 3. 确认所有关节都在限位内
+
+`robot_config.h` 里的每个关节都有：
+
+```text
+lower_limit_rad
+upper_limit_rad
+```
+
+新固件会在使能前检查当前反馈角。如果某个关节当前反馈不在限位附近，会拒绝使能并进入
+`ESTOP`，串口会打印类似：
+
+```text
+拒绝使能: joint=left_ankle_joint can_id=4 当前关节角 ... 超出限位 ...
+```
+
+这不是故障，而是在提醒零位还没有校准好。
+
+## 4. 单关节小步确认方向
+
+所有关节的中立姿态读数都进入合理范围后，再悬空执行单关节 jog。
+一次只测一个关节，角度从 `0.005rad` 或 `0.01rad` 开始。
+
+```bash
+python guguji_real_robot/host/scripts/joint_jog_test.py \
+  --deploy-config guguji_real_robot/host/configs/guguji_real_robot_bench.yaml \
+  --joint left_ankle_joint \
+  --delta-rad 0.01 \
+  --arm \
+  --confirm-calibrated \
+  --clear-faults
+```
+
+如果正向 jog 和仿真定义的正方向相反，把该关节 `direction` 改成 `-1.0f`，
+然后按照第 2 节公式重新确认 `zero_offset_rad`。
+
+## 5. 再跑整机策略
+
+只有在以下条件全部满足后，才运行 `run_real_policy.py --arm --confirm-calibrated`：
+
+- 机械结构已经修复，关节没有卡滞。
+- 只读遥测中 8 个关节都在限位范围内。
+- 8 个关节单独 jog 的方向都符合预期。
+- 机器人悬空或可靠支撑，电机电源可以立即断开。
+- 台架配置 `guguji_real_robot_bench.yaml` 下短时测试正常。
+
+首次策略测试仍然只跑 1~3 秒，不要直接落地行走。

guguji_real_robot/docs/rs00_mit_protocol_notes.md

@@ -22,6 +22,17 @@
 
 本固件在收到主机 `ARM` 命令后会先发 MIT 模式设置，再发使能；收到 `DISARM`、`ESTOP` 或命令超时后发送停止帧。
 
+## 2.1. 主动上报
+
+RS00 主动上报默认关闭。固件启动后会尝试对每个 `robot_config.h` 中配置的 CAN ID 发送：
+
+```text
+开启主动上报: FF FF FF FF FF FF 01 F9
+关闭主动上报: FF FF FF FF FF FF 00 F9
+```
+
+该命令需要电机固件支持 MIT 指令 13，不会使能电机。如果电机固件较旧或主动上报未生效，固件在 `DISARM | HOLD` 通信测试期间会周期发送停止帧，利用停止帧的应答来刷新反馈。
+
 ## 3. MIT 动态控制帧
 
 发送给目标电机 CAN ID，8 字节数据打包：

guguji_real_robot/firmware/esp32s3_espidf/README.md

@@ -22,7 +22,8 @@
 - WiFi AP：`guguji-robot` / `guguji1234`
 - UDP 命令端口：`7777`
 - UDP 遥测回传：发给最近一次命令来源地址
-- TWAI/CAN：1Mbps，控制 RS00 MIT 协议电机
+- TWAI/CAN：`GPIO41=TXD`、`GPIO40=RXD`，1Mbps，控制 RS00 MIT 协议电机
+- RS00 反馈：启动后尝试开启主动上报；失能通信测试时周期发送停止帧触发安全反馈
 - I2C：`GPIO10=SDA`、`GPIO11=SCL`，读取 QMI8658A 和 QMC6309
 - GPIO42：无源蜂鸣器状态提示
 - GPIO45：WS2812 RGB LED 状态提示
@@ -115,8 +116,12 @@ python3 host/scripts/ota_update.py \
 - 蓝灯慢闪：等待主机
 - 青色慢闪：失能/安全保持
 - 绿灯常亮：运行中
-- 黄灯双闪：命令超时
+- 黄灯双闪：运行使能后主机命令超时
 - 红灯快闪：急停或电机故障
 - 紫灯慢闪：传感器离线或反馈超时警告
 - 蓝白快闪：OTA 正在升级
 - 橙灯快闪：OTA 升级失败
+
+如果遥测显示 `fault_mask=0x0000FF00`，代表 8 个 RS00 都没有反馈。优先检查电机动力电、CANH/CANL、GND 共地、两端 120Ω 终端电阻、1Mbps 波特率、MIT 协议，以及每个电机 CAN ID 是否与 `main/robot_config.h` 一致。固件启动日志中如果出现“发送主动上报配置失败”或“发送停止帧失败”，通常表示 CAN 总线没有收到 ACK。
+
+固件会每 2 秒打印 `CAN诊断`。`raw_rx=0` 表示 ESP32 没收到任何标准 CAN 数据帧；`raw_rx>0` 但 `parsed_feedback=0` 通常表示收到了非当前 RS00 MIT 反馈格式或未配置 CAN ID 的帧。

guguji_real_robot/firmware/esp32s3_espidf/main/app_main.c

@@ -1,5 +1,7 @@
 #include <errno.h>
 #include <inttypes.h>
+#include <math.h>
+#include <stdio.h>
 #include <string.h>
 #include <unistd.h>
 
@@ -91,10 +93,231 @@ static float motor_to_joint_angle(int index, float motor_angle)
     return (motor_angle - joint->zero_offset_rad) * joint->direction;
 }
 
+static bool feedback_is_safe_for_arm(void)
+{
+    const uint32_t current_ms = now_ms();
+    static uint32_t last_log_ms = 0;
+    const bool should_log = current_ms - last_log_ms > 1000;
+    bool safe = true;
+
+    xSemaphoreTake(g_feedback_mutex, portMAX_DELAY);
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        const guguji_joint_config_t *joint = &GUGUJI_JOINTS[i];
+        const rs00_feedback_t feedback = g_feedback[i];
+        const bool feedback_stale =
+            feedback.last_feedback_ms == 0 ||
+            current_ms - feedback.last_feedback_ms > GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS;
+        const float joint_angle = motor_to_joint_angle(i, feedback.position_rad);
+        const bool outside_limit =
+            joint_angle < joint->lower_limit_rad - GUGUJI_ARM_POSITION_LIMIT_MARGIN_RAD ||
+            joint_angle > joint->upper_limit_rad + GUGUJI_ARM_POSITION_LIMIT_MARGIN_RAD;
+
+        if (!feedback_stale && !outside_limit) {
+            continue;
+        }
+
+        safe = false;
+        if (should_log) {
+            if (feedback_stale) {
+                ESP_LOGE(TAG,
+                         "拒绝使能: joint=%s can_id=%u 反馈过期或尚未收到",
+                         joint->name,
+                         joint->can_id);
+            } else {
+                ESP_LOGE(TAG,
+                         "拒绝使能: joint=%s can_id=%u 当前关节角 %.3f rad 超出限位 %.3f~%.3f rad "
+                         "(motor=%.3f zero=%.3f direction=%.1f)",
+                         joint->name,
+                         joint->can_id,
+                         joint_angle,
+                         joint->lower_limit_rad,
+                         joint->upper_limit_rad,
+                         feedback.position_rad,
+                         joint->zero_offset_rad,
+                         joint->direction);
+            }
+        }
+    }
+    xSemaphoreGive(g_feedback_mutex);
+
+    if (!safe && should_log) {
+        last_log_ms = current_ms;
+    }
+    return safe;
+}
+
+static bool command_targets_are_near_feedback(const guguji_command_packet_t *command)
+{
+    const uint32_t current_ms = now_ms();
+    static uint32_t last_log_ms = 0;
+    const bool should_log = current_ms - last_log_ms > 1000;
+    bool safe = true;
+
+    xSemaphoreTake(g_feedback_mutex, portMAX_DELAY);
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        const guguji_joint_config_t *joint = &GUGUJI_JOINTS[i];
+        const rs00_feedback_t feedback = g_feedback[i];
+        const bool feedback_stale =
+            feedback.last_feedback_ms == 0 ||
+            current_ms - feedback.last_feedback_ms > GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS;
+        if (feedback_stale) {
+            safe = false;
+            if (should_log) {
+                ESP_LOGE(TAG,
+                         "拒绝控制: joint=%s can_id=%u 反馈过期或尚未收到",
+                         joint->name,
+                         joint->can_id);
+            }
+            continue;
+        }
+
+        const float current_joint_angle = motor_to_joint_angle(i, feedback.position_rad);
+        const float requested_joint_target = command->positions_rad[i];
+        const float clamped_joint_target = clampf_local(
+            requested_joint_target,
+            joint->lower_limit_rad,
+            joint->upper_limit_rad);
+        const float error = fabsf(clamped_joint_target - current_joint_angle);
+        if (error <= GUGUJI_MAX_COMMAND_POSITION_ERROR_RAD) {
+            continue;
+        }
+
+        safe = false;
+        if (should_log) {
+            ESP_LOGE(TAG,
+                     "拒绝控制: joint=%s can_id=%u 目标跳变过大 target=%.3f current=%.3f "
+                     "error=%.3f max=%.3f",
+                     joint->name,
+                     joint->can_id,
+                     clamped_joint_target,
+                     current_joint_angle,
+                     error,
+                     GUGUJI_MAX_COMMAND_POSITION_ERROR_RAD);
+        }
+    }
+    xSemaphoreGive(g_feedback_mutex);
+
+    if (!safe && should_log) {
+        last_log_ms = current_ms;
+    }
+    return safe;
+}
+
+static const char *twai_state_name(int state)
+{
+    switch (state) {
+    case 0:
+        return "active";
+    case 1:
+        return "warning";
+    case 2:
+        return "passive";
+    case 3:
+        return "bus_off";
+    default:
+        return "unknown";
+    }
+}
+
+static void build_joint_mask_names(uint32_t joint_mask, char *buffer, size_t buffer_size)
+{
+    if (buffer_size == 0) {
+        return;
+    }
+
+    size_t offset = 0;
+    buffer[0] = '\0';
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        if ((joint_mask & (1u << i)) == 0) {
+            continue;
+        }
+
+        const int written = snprintf(
+            buffer + offset,
+            buffer_size - offset,
+            "%s%s",
+            offset > 0 ? "," : "",
+            GUGUJI_JOINTS[i].name);
+        if (written < 0) {
+            buffer[0] = '\0';
+            return;
+        }
+        if ((size_t)written >= buffer_size - offset) {
+            buffer[buffer_size - 1] = '\0';
+            return;
+        }
+        offset += (size_t)written;
+    }
+
+    if (offset == 0) {
+        snprintf(buffer, buffer_size, "none");
+    }
+}
+
+static void log_fault_mask_if_changed(uint32_t fault_mask)
+{
+    static uint32_t last_logged_fault_mask = UINT32_MAX;
+    if (fault_mask == last_logged_fault_mask) {
+        return;
+    }
+    last_logged_fault_mask = fault_mask;
+
+    if (fault_mask == 0) {
+        ESP_LOGI(TAG, "故障状态恢复: fault_mask=0x%08" PRIX32, fault_mask);
+        return;
+    }
+
+    char motor_faults[192] = {0};
+    char feedback_timeouts[192] = {0};
+    build_joint_mask_names(fault_mask & GUGUJI_FAULT_MOTOR_MASK, motor_faults, sizeof(motor_faults));
+    build_joint_mask_names((fault_mask & GUGUJI_FAULT_FEEDBACK_TIMEOUT_MASK) >> 8, feedback_timeouts, sizeof(feedback_timeouts));
+
+    ESP_LOGW(
+        TAG,
+        "故障状态变化: fault_mask=0x%08" PRIX32
+        " motor_fault=%s feedback_timeout=%s imu_offline=%s mag_offline=%s",
+        fault_mask,
+        motor_faults,
+        feedback_timeouts,
+        (fault_mask & GUGUJI_FAULT_IMU_OFFLINE) != 0 ? "yes" : "no",
+        (fault_mask & GUGUJI_FAULT_MAG_OFFLINE) != 0 ? "yes" : "no");
+}
+
 static void stop_all_motors(void)
 {
+    static uint32_t last_error_log_ms = 0;
+    const uint32_t current_ms = now_ms();
+    const bool log_errors = current_ms - last_error_log_ms > 1000;
+    bool logged_error = false;
+
     for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
-        rs00_mit_send_stop(GUGUJI_JOINTS[i].can_id);
+        const esp_err_t err = rs00_mit_send_stop(GUGUJI_JOINTS[i].can_id);
+        if (err != ESP_OK && log_errors) {
+            ESP_LOGW(TAG, "发送停止帧失败: joint=%s can_id=%u err=%s",
+                     GUGUJI_JOINTS[i].name,
+                     GUGUJI_JOINTS[i].can_id,
+                     esp_err_to_name(err));
+            logged_error = true;
+        }
+    }
+    if (logged_error) {
+        last_error_log_ms = current_ms;
+    }
+}
+
+static void request_active_reports(bool enabled)
+{
+    ESP_LOGI(TAG, "%s RS00 主动上报", enabled ? "开启" : "关闭");
+    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+        const esp_err_t err = rs00_mit_send_active_report(GUGUJI_JOINTS[i].can_id, enabled);
+        if (err != ESP_OK) {
+            ESP_LOGW(TAG, "发送主动上报配置失败: joint=%s can_id=%u enabled=%d err=%s",
+                     GUGUJI_JOINTS[i].name,
+                     GUGUJI_JOINTS[i].can_id,
+                     enabled ? 1 : 0,
+                     esp_err_to_name(err));
+        }
+        vTaskDelay(pdMS_TO_TICKS(2));
     }
 }
 
@@ -118,7 +341,13 @@ static bool prepare_for_ota(void *context)
 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);
+        const esp_err_t err = rs00_mit_send_clear_fault(GUGUJI_JOINTS[i].can_id);
+        if (err != ESP_OK) {
+            ESP_LOGW(TAG, "发送清错帧失败: joint=%s can_id=%u err=%s",
+                     GUGUJI_JOINTS[i].name,
+                     GUGUJI_JOINTS[i].can_id,
+                     esp_err_to_name(err));
+        }
     }
 }
 
@@ -126,9 +355,21 @@ 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);
+        esp_err_t err = rs00_mit_send_set_mode(GUGUJI_JOINTS[i].can_id, 0);
+        if (err != ESP_OK) {
+            ESP_LOGW(TAG, "发送 MIT 模式帧失败: joint=%s can_id=%u err=%s",
+                     GUGUJI_JOINTS[i].name,
+                     GUGUJI_JOINTS[i].can_id,
+                     esp_err_to_name(err));
+        }
         vTaskDelay(pdMS_TO_TICKS(2));
-        rs00_mit_send_enable(GUGUJI_JOINTS[i].can_id);
+        err = rs00_mit_send_enable(GUGUJI_JOINTS[i].can_id);
+        if (err != ESP_OK) {
+            ESP_LOGW(TAG, "发送使能帧失败: joint=%s can_id=%u err=%s",
+                     GUGUJI_JOINTS[i].name,
+                     GUGUJI_JOINTS[i].can_id,
+                     esp_err_to_name(err));
+        }
         vTaskDelay(pdMS_TO_TICKS(2));
     }
 }
@@ -272,6 +513,7 @@ static void udp_receive_task(void *arg)
 static void can_feedback_task(void *arg)
 {
     (void)arg;
+    uint32_t last_unknown_id_log_ms = 0;
     while (true) {
         rs00_feedback_t feedback = {0};
         if (!rs00_mit_poll_feedback(&feedback, 10)) {
@@ -279,6 +521,12 @@ static void can_feedback_task(void *arg)
         }
         const int joint_index = find_joint_by_motor_id(feedback.motor_id);
         if (joint_index < 0) {
+            const uint32_t current_ms = now_ms();
+            if (current_ms - last_unknown_id_log_ms > 1000) {
+                ESP_LOGW(TAG, "收到未配置的 RS00 反馈: motor_id=%u，请检查 robot_config.h CAN ID",
+                         feedback.motor_id);
+                last_unknown_id_log_ms = current_ms;
+            }
             continue;
         }
 
@@ -288,6 +536,53 @@ static void can_feedback_task(void *arg)
     }
 }
 
+static void can_diagnostics_task(void *arg)
+{
+    (void)arg;
+    while (true) {
+        rs00_mit_diagnostics_t diagnostics = {0};
+        const esp_err_t err = rs00_mit_get_diagnostics(&diagnostics);
+        if (err == ESP_OK) {
+            char id_counts[160] = {0};
+            size_t offset = 0;
+            for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
+                const uint8_t can_id = GUGUJI_JOINTS[i].can_id;
+                const uint32_t count = can_id <= RS00_MIT_TRACKED_ID_MAX
+                    ? diagnostics.feedback_count_by_id[can_id]
+                    : 0;
+                const int written = snprintf(
+                    id_counts + offset,
+                    sizeof(id_counts) - offset,
+                    "%s%u:%" PRIu32,
+                    i > 0 ? " " : "",
+                    can_id,
+                    count);
+                if (written < 0 || (size_t)written >= sizeof(id_counts) - offset) {
+                    id_counts[sizeof(id_counts) - 1] = '\0';
+                    break;
+                }
+                offset += (size_t)written;
+            }
+
+            ESP_LOGI(TAG,
+                     "CAN诊断: state=%s tx_err=%u rx_err=%u bus_err=%" PRIu32
+                     " raw_rx=%" PRIu32 " parsed_feedback=%" PRIu32 " invalid_rx=%" PRIu32
+                     " id_counts=[%s]",
+                     twai_state_name(diagnostics.twai_state),
+                     diagnostics.tx_error_count,
+                     diagnostics.rx_error_count,
+                     diagnostics.bus_error_count,
+                     diagnostics.raw_rx_count,
+                     diagnostics.parsed_feedback_count,
+                     diagnostics.invalid_rx_count,
+                     id_counts);
+        } else {
+            ESP_LOGW(TAG, "读取 CAN 诊断失败: %s", esp_err_to_name(err));
+        }
+        vTaskDelay(pdMS_TO_TICKS(2000));
+    }
+}
+
 static void sensor_task(void *arg)
 {
     (void)arg;
@@ -310,6 +605,7 @@ static void control_task(void *arg)
     (void)arg;
     bool motors_enabled = false;
     uint32_t last_clear_fault_sequence = UINT32_MAX;
+    uint32_t last_disarm_feedback_poll_ms = 0;
 
     while (true) {
         if (guguji_ota_is_active()) {
@@ -335,11 +631,28 @@ static void control_task(void *arg)
         xSemaphoreGive(g_command_mutex);
 
         if (!have_command || command_age_ms > GUGUJI_COMMAND_TIMEOUT_MS) {
+            const bool stale_command_was_estop =
+                have_command && (command.flags & GUGUJI_COMMAND_FLAG_ESTOP) != 0;
+            const bool stale_command_was_armed =
+                have_command &&
+                (command.flags & GUGUJI_COMMAND_FLAG_ARM) != 0 &&
+                (command.flags & GUGUJI_COMMAND_FLAG_DISARM) == 0 &&
+                !stale_command_was_estop;
+            const bool motors_were_enabled = motors_enabled;
             if (motors_enabled) {
                 stop_all_motors();
                 motors_enabled = false;
             }
-            g_robot_state = have_command ? GUGUJI_STATE_TIMEOUT : GUGUJI_STATE_DISARMED;
+            if (!have_command) {
+                g_robot_state = GUGUJI_STATE_DISARMED;
+            } else if (stale_command_was_estop) {
+                g_robot_state = GUGUJI_STATE_ESTOP;
+            } else if (stale_command_was_armed || motors_were_enabled) {
+                g_robot_state = GUGUJI_STATE_TIMEOUT;
+            } else {
+                // 通信测试或主动失能后，主机停止发送保持包属于正常结束，不触发黄色超时报警。
+                g_robot_state = GUGUJI_STATE_DISARMED;
+            }
             vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
             continue;
         }
@@ -364,11 +677,27 @@ static void control_task(void *arg)
                 stop_all_motors();
                 motors_enabled = false;
             }
+            const uint32_t current_ms = now_ms();
+            if (current_ms - last_disarm_feedback_poll_ms >= GUGUJI_DISARM_FEEDBACK_POLL_MS) {
+                // RS00 主动上报默认可能关闭；失能时发送停止帧可安全触发反馈应答。
+                stop_all_motors();
+                last_disarm_feedback_poll_ms = current_ms;
+            }
             g_robot_state = GUGUJI_STATE_DISARMED;
             vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
             continue;
         }
 
+        if (!feedback_is_safe_for_arm() || !command_targets_are_near_feedback(&command)) {
+            if (motors_enabled) {
+                stop_all_motors();
+                motors_enabled = false;
+            }
+            g_robot_state = GUGUJI_STATE_ESTOP;
+            vTaskDelay(pdMS_TO_TICKS(GUGUJI_CONTROL_PERIOD_MS));
+            continue;
+        }
+
         if (!motors_enabled) {
             enable_all_motors();
             motors_enabled = true;
@@ -468,6 +797,7 @@ static void telemetry_task(void *arg)
 
         g_latest_fault_mask = fault_mask;
         packet.fault_mask = fault_mask;
+        log_fault_mask_if_changed(fault_mask);
 
         guguji_finalize_telemetry_packet(&packet);
         sendto(sock, &packet, sizeof(packet), 0, (struct sockaddr *)&peer_addr, peer_len);
@@ -546,10 +876,12 @@ void app_main(void)
     wifi_init();
     ESP_ERROR_CHECK(guguji_sensors_init());
     ESP_ERROR_CHECK(rs00_mit_init(GUGUJI_TWAI_TX_GPIO, GUGUJI_TWAI_RX_GPIO));
+    request_active_reports(true);
 
     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(can_diagnostics_task, "can_diag", 4096, NULL, 4, 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);

guguji_real_robot/firmware/esp32s3_espidf/main/config.h

@@ -22,8 +22,8 @@
 // ESP32S3 引脚配置
 // ==============================
 // 控制板上已经集成 TJA1050T CAN 总线收发器，下面两个引脚连接到收发器 TXD/RXD。
-#define GUGUJI_TWAI_TX_GPIO GPIO_NUM_5
-#define GUGUJI_TWAI_RX_GPIO GPIO_NUM_4
+#define GUGUJI_TWAI_TX_GPIO GPIO_NUM_41
+#define GUGUJI_TWAI_RX_GPIO GPIO_NUM_40
 
 // 无屏状态提示硬件。
 #define GUGUJI_BUZZER_GPIO GPIO_NUM_42
@@ -47,3 +47,9 @@
 #define GUGUJI_TELEMETRY_PERIOD_MS 20
 #define GUGUJI_COMMAND_TIMEOUT_MS 200
 #define GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS 500
+// 失能通信测试时，周期性发送停止帧触发 RS00 应答反馈；不会使能或驱动电机。
+#define GUGUJI_DISARM_FEEDBACK_POLL_MS 250
+// 使能前必须确认当前反馈角度在配置限位附近，避免零位未校准时被限位夹取成大幅跳变。
+#define GUGUJI_ARM_POSITION_LIMIT_MARGIN_RAD 0.05f
+// 单帧目标与当前反馈位置差值过大时拒绝执行，防止主机配置错误或零位错误导致撞限位。
+#define GUGUJI_MAX_COMMAND_POSITION_ERROR_RAD 0.25f

guguji_real_robot/firmware/esp32s3_espidf/main/rs00_mit.c

@@ -19,6 +19,10 @@ typedef struct {
 
 static twai_node_handle_t g_twai_node = NULL;
 static QueueHandle_t g_rx_queue = NULL;
+static volatile uint32_t g_raw_rx_count = 0;
+static volatile uint32_t g_parsed_feedback_count = 0;
+static volatile uint32_t g_invalid_rx_count = 0;
+static volatile uint32_t g_feedback_count_by_id[RS00_MIT_TRACKED_ID_MAX + 1] = {0};
 
 static float clampf(float value, float min_value, float max_value)
 {
@@ -85,6 +89,7 @@ static bool IRAM_ATTR rs00_rx_done_callback(
         received.identifier = frame.header.id;
         received.data_length = (uint8_t)frame.header.dlc;
         received.is_extended = frame.header.ide != 0;
+        ++g_raw_rx_count;
         xQueueSendFromISR(g_rx_queue, &received, &higher_priority_task_woken);
     }
     return higher_priority_task_woken == pdTRUE;
@@ -180,6 +185,13 @@ esp_err_t rs00_mit_send_set_mode(uint8_t motor_id, uint8_t mode)
     return transmit_standard(motor_id, data);
 }
 
+esp_err_t rs00_mit_send_active_report(uint8_t motor_id, bool enabled)
+{
+    // RS00 MIT 指令 13：主动上报。0=关闭，1=开启；该命令不使能电机。
+    uint8_t data[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, enabled ? 0x01 : 0x00, 0xF9};
+    return transmit_standard(motor_id, data);
+}
+
 esp_err_t rs00_mit_send_control(
     uint8_t motor_id,
     float position_rad,
@@ -214,6 +226,7 @@ bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms)
         return false;
     }
     if (frame.is_extended || frame.data_length != 8) {
+        ++g_invalid_rx_count;
         return false;
     }
 
@@ -232,5 +245,38 @@ bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms)
     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);
+    ++g_parsed_feedback_count;
+    if (feedback->motor_id <= RS00_MIT_TRACKED_ID_MAX) {
+        ++g_feedback_count_by_id[feedback->motor_id];
+    }
     return true;
 }
+
+esp_err_t rs00_mit_get_diagnostics(rs00_mit_diagnostics_t *diagnostics)
+{
+    if (diagnostics == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+    if (g_twai_node == NULL) {
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    twai_node_status_t status = {0};
+    twai_node_record_t record = {0};
+    const esp_err_t err = twai_node_get_info(g_twai_node, &status, &record);
+    if (err != ESP_OK) {
+        return err;
+    }
+
+    diagnostics->raw_rx_count = g_raw_rx_count;
+    diagnostics->parsed_feedback_count = g_parsed_feedback_count;
+    diagnostics->invalid_rx_count = g_invalid_rx_count;
+    diagnostics->twai_state = (int)status.state;
+    diagnostics->tx_error_count = status.tx_error_count;
+    diagnostics->rx_error_count = status.rx_error_count;
+    diagnostics->bus_error_count = record.bus_err_num;
+    for (int i = 0; i <= RS00_MIT_TRACKED_ID_MAX; ++i) {
+        diagnostics->feedback_count_by_id[i] = g_feedback_count_by_id[i];
+    }
+    return ESP_OK;
+}

guguji_real_robot/firmware/esp32s3_espidf/main/rs00_mit.h

@@ -6,6 +6,8 @@
 #include "driver/gpio.h"
 #include "esp_err.h"
 
+#define RS00_MIT_TRACKED_ID_MAX 127
+
 typedef struct {
     uint8_t motor_id;
     float position_rad;
@@ -18,11 +20,23 @@ typedef struct {
     uint32_t last_feedback_ms;
 } rs00_feedback_t;
 
+typedef struct {
+    uint32_t raw_rx_count;
+    uint32_t parsed_feedback_count;
+    uint32_t invalid_rx_count;
+    int twai_state;
+    uint16_t tx_error_count;
+    uint16_t rx_error_count;
+    uint32_t bus_error_count;
+    uint32_t feedback_count_by_id[RS00_MIT_TRACKED_ID_MAX + 1];
+} rs00_mit_diagnostics_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_active_report(uint8_t motor_id, bool enabled);
 esp_err_t rs00_mit_send_control(
     uint8_t motor_id,
     float position_rad,
@@ -31,3 +45,4 @@ esp_err_t rs00_mit_send_control(
     float kd,
     float torque_ff_nm);
 bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms);
+esp_err_t rs00_mit_get_diagnostics(rs00_mit_diagnostics_t *diagnostics);

guguji_real_robot/host/README.md

@@ -2,7 +2,16 @@
 
 这个目录放主机端部署代码：本机加载训练好的 PPO 模型，通过 WiFi UDP 把 8 个关节目标角发送给 ESP32S3。
 
-最小运行命令：
+只读遥测命令，不会使能电机：
+
+```bash
+cd /home/corvin/Project/guguji_simulation
+python guguji_real_robot/host/scripts/read_robot_state.py \
+  --deploy-config guguji_real_robot/host/configs/guguji_real_robot_bench.yaml \
+  --samples 3
+```
+
+模型部署命令：
 
 ```bash
 cd /home/corvin/Project/guguji_simulation
@@ -15,7 +24,19 @@ python guguji_real_robot/host/scripts/run_real_policy.py \
   --deterministic
 ```
 
-确认机器人悬空、急停和电源都准备好后，再加 `--arm` 让 ESP32S3 使能电机。
+确认机器人悬空、急停和电源都准备好，并且已经逐关节校准零位和方向后，再加
+`--arm --confirm-calibrated` 让 ESP32S3 使能电机。
+
+单关节方向校准请先使用：
+
+```bash
+python guguji_real_robot/host/scripts/joint_jog_test.py \
+  --deploy-config guguji_real_robot/host/configs/guguji_real_robot_bench.yaml \
+  --joint left_ankle_joint \
+  --delta-rad 0.01 \
+  --arm \
+  --confirm-calibrated
+```
 
 固件 OTA 升级工具位于 `scripts/ota_update.py`，详细教程见
 [`../docs/ota_upgrade_guide.md`](../docs/ota_upgrade_guide.md)。

guguji_real_robot/host/configs/guguji_real_robot_bench.yaml

@@ -0,0 +1,36 @@
+# guguji 真机台架/悬空首测配置
+# 目的：第一次给电机使能时使用更低增益、更小单步变化，先确认零位、方向和通信稳定性。
+
+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:
+  # 顺序必须与 firmware/main/robot_config.h 中 guguji_joint_config_t 数组一致。
+  # 低 kp/kd 只用于台架/悬空验证；站立或行走前再逐步调高。
+  kp: [1.5, 1.5, 1.2, 0.8, 1.5, 1.5, 1.2, 0.8]
+  kd: [0.08, 0.08, 0.06, 0.05, 0.08, 0.08, 0.06, 0.05]
+  torque_ff: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+safety:
+  # 每帧关节目标最大变化量，首测时尽量保守。
+  max_joint_step_rad: 0.002
+  max_roll_rad: 0.35
+  max_pitch_rad: 0.35
+  max_joint_velocity_rad_s: 0.8
+  arm_position_limit_margin_rad: 0.05
+  run_position_limit_margin_rad: 0.08
+  max_motor_temp_c: 70.0
+  min_battery_v: 9.0
+
+state_estimation:
+  estimated_base_height_m: 0.35
+  estimated_forward_velocity_m_s: 0.0
+  target_forward_velocity_m_s: 0.0
+
+logging:
+  status_interval_s: 0.2

guguji_real_robot/host/scripts/joint_jog_test.py

@@ -0,0 +1,250 @@
+#!/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,
+    CommandPacket,
+    TelemetryPacket,
+    UdpRobotLink,
+)
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description='Jog one joint by a tiny offset for direction calibration.')
+    parser.add_argument('--deploy-config', default=str(HOST_ROOT / 'configs' / 'guguji_real_robot_bench.yaml'))
+    parser.add_argument('--rl-config', default=str(PROJECT_ROOT / 'guguji_rl' / 'configs' / 'balance_ppo.yaml'))
+    parser.add_argument('--joint', required=True, help='关节名，或 1~8 的序号')
+    parser.add_argument('--delta-rad', type=float, default=0.015, help='小步测试角度，建议 0.005~0.02rad')
+    parser.add_argument('--duration', type=float, default=0.8, help='保持小步目标的时间，单位秒')
+    parser.add_argument('--ramp-seconds', type=float, default=0.6, help='从当前位置渐变到目标的时间，单位秒')
+    parser.add_argument('--settle-seconds', type=float, default=0.6, help='小步前后保持当前位置的时间，单位秒')
+    parser.add_argument('--arm', action='store_true', help='真正使能并执行 jog；不加时只打印遥测')
+    parser.add_argument('--confirm-calibrated', action='store_true', help='确认零位已在限位内后允许使能')
+    parser.add_argument('--clear-faults', action='store_true', help='启动时请求 ESP32S3 清除 RS00 故障')
+    return parser.parse_args()
+
+
+def joint_index_from_arg(joint: str, joint_names: list[str]) -> int:
+    if joint.isdigit():
+        index = int(joint) - 1
+        if 0 <= index < len(joint_names):
+            return index
+    if joint in joint_names:
+        return joint_names.index(joint)
+    raise ValueError(f'未知关节 {joint!r}，可选: ' + ', '.join(joint_names))
+
+
+def send_command(
+    link: UdpRobotLink,
+    *,
+    sequence: int,
+    positions_rad: np.ndarray,
+    kp: np.ndarray,
+    kd: np.ndarray,
+    flags: int,
+) -> int:
+    zero = np.zeros_like(positions_rad, dtype=np.float32)
+    link.send_command(
+        CommandPacket(
+            sequence=sequence,
+            positions_rad=positions_rad.astype(np.float32),
+            velocities_rad_s=zero,
+            kp=kp.astype(np.float32),
+            kd=kd.astype(np.float32),
+            torque_ff_nm=zero,
+            flags=flags,
+        )
+    )
+    return (sequence + 1) & 0xFFFFFFFF
+
+
+def send_stop(
+    link: UdpRobotLink,
+    *,
+    sequence: int,
+    positions_rad: np.ndarray,
+    repeats: int = 10,
+) -> int:
+    zero = np.zeros_like(positions_rad, dtype=np.float32)
+    for _ in range(repeats):
+        sequence = send_command(
+            link,
+            sequence=sequence,
+            positions_rad=positions_rad,
+            kp=zero,
+            kd=zero,
+            flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD | COMMAND_FLAG_ESTOP,
+        )
+        time.sleep(0.02)
+    return sequence
+
+
+def assert_safe(
+    telemetry: TelemetryPacket,
+    *,
+    joint_names: list[str],
+    max_roll_rad: float,
+    max_pitch_rad: float,
+    max_joint_velocity_rad_s: float,
+) -> None:
+    if telemetry.fault_mask != 0:
+        raise RuntimeError(f'ESP32S3 fault_mask=0x{telemetry.fault_mask:08X}')
+    if abs(telemetry.roll) > max_roll_rad:
+        raise RuntimeError(f'roll 超限 {telemetry.roll:.3f}rad')
+    if abs(telemetry.pitch) > max_pitch_rad:
+        raise RuntimeError(f'pitch 超限 {telemetry.pitch:.3f}rad')
+    max_velocity_index = int(np.argmax(np.abs(telemetry.joint_velocity_rad_s)))
+    max_velocity = float(abs(telemetry.joint_velocity_rad_s[max_velocity_index]))
+    if max_velocity > max_joint_velocity_rad_s:
+        raise RuntimeError(
+            f'{joint_names[max_velocity_index]} 速度超限 '
+            f'{max_velocity:.3f}rad/s > {max_joint_velocity_rad_s:.3f}rad/s'
+        )
+
+
+def print_joint_snapshot(telemetry: TelemetryPacket, joint_names: list[str], selected_index: int) -> None:
+    print(
+        f'{joint_names[selected_index]}: '
+        f'pos={telemetry.joint_position_rad[selected_index]:.4f}rad '
+        f'vel={telemetry.joint_velocity_rad_s[selected_index]:.4f}rad/s '
+        f'state={telemetry.robot_state_name} fault=0x{telemetry.fault_mask:08X}'
+    )
+
+
+def main() -> int:
+    args = parse_args()
+    if abs(args.delta_rad) > 0.03:
+        print('安全拦截：--delta-rad 绝对值不能超过 0.03rad。')
+        return 2
+    if args.arm and not args.confirm_calibrated:
+        print('安全拦截：执行 jog 需要同时添加 --arm 和 --confirm-calibrated。')
+        return 2
+
+    deploy_config = load_yaml(args.deploy_config)
+    network_config = deploy_config['network']
+    control_config = deploy_config['control']
+    safety_config = deploy_config['safety']
+    mapper = DeployActionMapper(rl_config_path=args.rl_config, deploy_config=deploy_config['state_estimation'])
+    selected_index = joint_index_from_arg(args.joint, mapper.joint_names)
+
+    kp = np.asarray(control_config['kp'], dtype=np.float32)
+    kd = np.asarray(control_config['kd'], dtype=np.float32)
+    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)),
+    )
+
+    sequence = 0
+    hold = mapper.nominal_joint_targets.astype(np.float32)
+    max_roll_rad = float(safety_config['max_roll_rad'])
+    max_pitch_rad = float(safety_config['max_pitch_rad'])
+    max_joint_velocity_rad_s = float(safety_config.get('max_joint_velocity_rad_s', 0.8))
+
+    try:
+        for _ in range(5):
+            sequence = send_command(
+                link,
+                sequence=sequence,
+                positions_rad=hold,
+                kp=np.zeros_like(kp),
+                kd=np.zeros_like(kd),
+                flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD,
+            )
+            time.sleep(0.05)
+
+        telemetry = link.receive_telemetry()
+        hold = telemetry.joint_position_rad.astype(np.float32).copy()
+        print('当前反馈:')
+        print_joint_snapshot(telemetry, mapper.joint_names, selected_index)
+        if not args.arm:
+            print('未添加 --arm，只读取遥测，不执行 jog。')
+            return 0
+
+        flags = COMMAND_FLAG_ARM
+        if args.clear_faults:
+            flags |= COMMAND_FLAG_CLEAR_FAULTS
+
+        start = time.monotonic()
+        while time.monotonic() - start < max(args.settle_seconds, 0.0):
+            sequence = send_command(link, sequence=sequence, positions_rad=hold, kp=kp, kd=kd, flags=flags)
+            telemetry = link.receive_telemetry()
+            assert_safe(
+                telemetry,
+                joint_names=mapper.joint_names,
+                max_roll_rad=max_roll_rad,
+                max_pitch_rad=max_pitch_rad,
+                max_joint_velocity_rad_s=max_joint_velocity_rad_s,
+            )
+
+        jog_target = hold.copy()
+        jog_target[selected_index] += float(args.delta_rad)
+        print(
+            f'开始 jog: {mapper.joint_names[selected_index]} '
+            f'{hold[selected_index]:.4f} -> {jog_target[selected_index]:.4f} rad'
+        )
+
+        start = time.monotonic()
+        total = max(args.ramp_seconds + args.duration, 0.1)
+        while time.monotonic() - start < total:
+            elapsed = time.monotonic() - start
+            progress = min(elapsed / max(args.ramp_seconds, 0.05), 1.0)
+            target = hold.copy()
+            target[selected_index] = hold[selected_index] + float(args.delta_rad) * progress
+            sequence = send_command(link, sequence=sequence, positions_rad=target, kp=kp, kd=kd, flags=COMMAND_FLAG_ARM)
+            telemetry = link.receive_telemetry()
+            assert_safe(
+                telemetry,
+                joint_names=mapper.joint_names,
+                max_roll_rad=max_roll_rad,
+                max_pitch_rad=max_pitch_rad,
+                max_joint_velocity_rad_s=max_joint_velocity_rad_s,
+            )
+            print_joint_snapshot(telemetry, mapper.joint_names, selected_index)
+
+        start = time.monotonic()
+        while time.monotonic() - start < max(args.settle_seconds, 0.0):
+            sequence = send_command(link, sequence=sequence, positions_rad=hold, kp=kp, kd=kd, flags=COMMAND_FLAG_ARM)
+            telemetry = link.receive_telemetry()
+            assert_safe(
+                telemetry,
+                joint_names=mapper.joint_names,
+                max_roll_rad=max_roll_rad,
+                max_pitch_rad=max_pitch_rad,
+                max_joint_velocity_rad_s=max_joint_velocity_rad_s,
+            )
+        print('jog 完成，已回到起始保持位置。')
+    except (KeyboardInterrupt, TimeoutError, RuntimeError) as exc:
+        print(f'测试中止: {exc}')
+        sequence = send_stop(link, sequence=sequence, positions_rad=hold)
+        return 4
+    finally:
+        send_stop(link, sequence=sequence, positions_rad=hold, repeats=5)
+        link.close()
+    return 0
+
+
+if __name__ == '__main__':
+    raise SystemExit(main())

guguji_real_robot/host/scripts/read_robot_state.py

@@ -0,0 +1,149 @@
+#!/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_DISARM,
+    COMMAND_FLAG_HOLD,
+    CommandPacket,
+    TelemetryPacket,
+    UdpRobotLink,
+)
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description='Read ESP32S3 telemetry without enabling motors.')
+    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' / 'balance_ppo.yaml'))
+    parser.add_argument('--samples', type=int, default=10, help='打印多少帧遥测')
+    parser.add_argument('--interval', type=float, default=0.2, help='打印间隔，单位秒')
+    return parser.parse_args()
+
+
+def format_fault_summary(telemetry: TelemetryPacket, joint_names: list[str]) -> str:
+    parts: list[str] = []
+    motor_faults = [name for index, name in enumerate(joint_names) if telemetry.motor_fault_mask & (1 << index)]
+    feedback_timeouts = [
+        name for index, name in enumerate(joint_names) if telemetry.feedback_timeout_mask & (1 << index)
+    ]
+    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 send_disarm_hold(
+    link: UdpRobotLink,
+    *,
+    sequence: int,
+    positions_rad: np.ndarray,
+    joint_count: int,
+) -> int:
+    zero = np.zeros(joint_count, dtype=np.float32)
+    link.send_command(
+        CommandPacket(
+            sequence=sequence,
+            positions_rad=positions_rad.astype(np.float32),
+            velocities_rad_s=zero,
+            kp=zero,
+            kd=zero,
+            torque_ff_nm=zero,
+            flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD,
+        )
+    )
+    return (sequence + 1) & 0xFFFFFFFF
+
+
+def print_sample(telemetry: TelemetryPacket, joint_names: list[str]) -> None:
+    rpy_deg = np.degrees(telemetry.rpy_rad)
+    print(
+        f'\n[ESP32S3] state={telemetry.robot_state_name} seq={telemetry.sequence} '
+        f'uptime={telemetry.uptime_ms / 1000.0:.1f}s 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'
+    )
+    print('index  joint_name                 pos_rad   vel_rad_s   temp_C')
+    for index, joint_name in enumerate(joint_names):
+        print(
+            f'{index + 1:>5}  {joint_name:<25} '
+            f'{telemetry.joint_position_rad[index]:>8.3f} '
+            f'{telemetry.joint_velocity_rad_s[index]:>10.3f} '
+            f'{telemetry.motor_temperature_c[index]:>8.1f}'
+        )
+
+
+def main() -> int:
+    args = parse_args()
+    deploy_config = load_yaml(args.deploy_config)
+    network_config = deploy_config['network']
+    mapper = DeployActionMapper(rl_config_path=args.rl_config, deploy_config=deploy_config['state_estimation'])
+    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)),
+    )
+
+    sequence = 0
+    hold = mapper.nominal_joint_targets.astype(np.float32)
+    try:
+        for _ in range(5):
+            sequence = send_disarm_hold(
+                link,
+                sequence=sequence,
+                positions_rad=hold,
+                joint_count=len(mapper.joint_names),
+            )
+            time.sleep(0.05)
+
+        printed = 0
+        while printed < args.samples:
+            sequence = send_disarm_hold(
+                link,
+                sequence=sequence,
+                positions_rad=hold,
+                joint_count=len(mapper.joint_names),
+            )
+            telemetry = link.receive_telemetry()
+            print_sample(telemetry, mapper.joint_names)
+            printed += 1
+            time.sleep(max(args.interval, 0.0))
+    except KeyboardInterrupt:
+        print('收到 Ctrl+C，已保持失能退出。')
+    finally:
+        for _ in range(3):
+            sequence = send_disarm_hold(
+                link,
+                sequence=sequence,
+                positions_rad=hold,
+                joint_count=len(mapper.joint_names),
+            )
+            time.sleep(0.02)
+        link.close()
+    return 0
+
+
+if __name__ == '__main__':
+    raise SystemExit(main())

guguji_real_robot/host/scripts/run_real_policy.py

@@ -41,6 +41,11 @@ def parse_args() -> argparse.Namespace:
     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 遥测日志间隔，单位秒')
+    parser.add_argument(
+        '--confirm-calibrated',
+        action='store_true',
+        help='确认已经逐关节校准零位和方向后，才允许 --arm 运行策略',
+    )
     return parser.parse_args()
 
 
@@ -74,6 +79,8 @@ def format_fault_summary(telemetry: TelemetryPacket, joint_names: list[str]) ->
 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)))
+    max_abs_velocity = float(np.max(np.abs(telemetry.joint_velocity_rad_s)))
+    max_velocity_index = int(np.argmax(np.abs(telemetry.joint_velocity_rad_s)))
     print(
         '[ESP32S3] '
         f'state={telemetry.robot_state_name} '
@@ -83,12 +90,65 @@ def print_telemetry_status(telemetry: TelemetryPacket, joint_names: list[str]) -
         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'
+        f'joint_vel_mean={mean_abs_velocity:.3f}rad/s '
+        f'joint_vel_max={max_abs_velocity:.3f}rad/s@{joint_names[max_velocity_index]}'
     )
 
 
+def find_out_of_range_joints(
+    positions_rad: np.ndarray,
+    lower: np.ndarray,
+    upper: np.ndarray,
+    joint_names: list[str],
+    *,
+    margin_rad: float,
+) -> list[str]:
+    violations: list[str] = []
+    for index, (position, low, high) in enumerate(zip(positions_rad, lower, upper)):
+        if position < low - margin_rad or position > high + margin_rad:
+            violations.append(
+                f'{joint_names[index]}={position:.3f}rad '
+                f'(limit {low:.3f}~{high:.3f}rad, margin {margin_rad:.3f})'
+            )
+    return violations
+
+
+def send_emergency_stop(
+    link: UdpRobotLink,
+    *,
+    sequence: int,
+    hold_positions: np.ndarray,
+    joint_count: int,
+    repeats: int = 12,
+) -> int:
+    zero = np.zeros(joint_count, dtype=np.float32)
+    positions = np.asarray(hold_positions, dtype=np.float32)
+    for _ in range(repeats):
+        link.send_command(
+            CommandPacket(
+                sequence=sequence,
+                positions_rad=positions,
+                velocities_rad_s=zero,
+                kp=zero,
+                kd=zero,
+                torque_ff_nm=zero,
+                flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD | COMMAND_FLAG_ESTOP,
+            )
+        )
+        sequence = (sequence + 1) & 0xFFFFFFFF
+        time.sleep(0.02)
+    return sequence
+
+
 def main() -> int:
     args = parse_args()
+    if args.arm and not args.confirm_calibrated:
+        print(
+            '安全拦截：当前命令包含 --arm，但没有 --confirm-calibrated。\n'
+            '请先逐个关节确认零位、方向和机械限位，确认后再显式添加 --confirm-calibrated。'
+        )
+        return 2
+
     deploy_config = load_yaml(args.deploy_config)
     network_config = deploy_config['network']
     control_config = deploy_config['control']
@@ -125,6 +185,12 @@ def main() -> int:
     last_state: int | None = None
     last_fault_mask: int | None = None
     last_safety_message: str | None = None
+    last_hold_positions = mapper.nominal_joint_targets.astype(np.float32)
+    arm_position_margin_rad = float(safety_config.get('arm_position_limit_margin_rad', 0.05))
+    run_position_margin_rad = float(safety_config.get('run_position_limit_margin_rad', arm_position_margin_rad))
+    max_joint_velocity_rad_s = safety_config.get('max_joint_velocity_rad_s')
+    if max_joint_velocity_rad_s is not None:
+        max_joint_velocity_rad_s = float(max_joint_velocity_rad_s)
 
     print(
         '主机部署配置: '
@@ -177,6 +243,7 @@ def main() -> int:
                 limiter.reset(telemetry.joint_position_rad)
                 mapper.reset()
                 first_telemetry = False
+                last_hold_positions = telemetry.joint_position_rad.astype(np.float32).copy()
                 start_time = time.monotonic()
                 next_tick = start_time
                 print('已收到遥测，开始部署循环。')
@@ -185,8 +252,37 @@ def main() -> int:
                 last_state = telemetry.robot_state
                 last_fault_mask = telemetry.fault_mask
 
+                if args.arm:
+                    arm_blockers: list[str] = []
+                    arm_blockers.extend(
+                        find_out_of_range_joints(
+                            telemetry.joint_position_rad,
+                            mapper.joint_lower,
+                            mapper.joint_upper,
+                            mapper.joint_names,
+                            margin_rad=arm_position_margin_rad,
+                        )
+                    )
+                    if abs(telemetry.roll) > float(safety_config['max_roll_rad']):
+                        arm_blockers.append(f'roll={telemetry.roll:.3f}rad 超过首帧限制')
+                    if abs(telemetry.pitch) > float(safety_config['max_pitch_rad']):
+                        arm_blockers.append(f'pitch={telemetry.pitch:.3f}rad 超过首帧限制')
+                    if arm_blockers:
+                        print('禁止使能：当前反馈状态不在安全范围内。')
+                        for item in arm_blockers:
+                            print(f'  - {item}')
+                        print('请先校准 robot_config.h 的 zero_offset_rad/direction，或重新设置电机机械零位。')
+                        sequence = send_emergency_stop(
+                            link,
+                            sequence=sequence,
+                            hold_positions=last_hold_positions,
+                            joint_count=len(mapper.joint_names),
+                        )
+                        return 3
+
             elapsed = time.monotonic() - start_time
             if args.max_seconds > 0 and elapsed >= args.max_seconds:
+                print(f'达到 max-seconds={args.max_seconds:.1f}s，发送失能保持命令后退出。')
                 break
 
             now = time.monotonic()
@@ -238,14 +334,42 @@ def main() -> int:
             if abs(telemetry.pitch) > float(safety_config['max_pitch_rad']):
                 flags |= COMMAND_FLAG_ESTOP
                 safety_messages.append(f'pitch 超限 {telemetry.pitch:.3f} rad')
+            if max_joint_velocity_rad_s is not None:
+                max_velocity_index = int(np.argmax(np.abs(telemetry.joint_velocity_rad_s)))
+                max_velocity = float(abs(telemetry.joint_velocity_rad_s[max_velocity_index]))
+                if max_velocity > max_joint_velocity_rad_s:
+                    flags |= COMMAND_FLAG_ESTOP
+                    safety_messages.append(
+                        f'{mapper.joint_names[max_velocity_index]} 速度超限 '
+                        f'{max_velocity:.3f} rad/s > {max_joint_velocity_rad_s:.3f} rad/s'
+                    )
+            position_violations = find_out_of_range_joints(
+                telemetry.joint_position_rad,
+                mapper.joint_lower,
+                mapper.joint_upper,
+                mapper.joint_names,
+                margin_rad=run_position_margin_rad,
+            )
+            if position_violations:
+                flags |= COMMAND_FLAG_ESTOP
+                safety_messages.append('关节位置越界: ' + '; '.join(position_violations))
 
             safety_message = '；'.join(safety_messages)
             if safety_message and safety_message != last_safety_message:
-                print(f'{safety_message}，已请求急停。')
+                print(f'{safety_message}，立即发送急停并退出。')
                 last_safety_message = safety_message
             elif not safety_message:
                 last_safety_message = None
 
+            if safety_message:
+                sequence = send_emergency_stop(
+                    link,
+                    sequence=sequence,
+                    hold_positions=telemetry.joint_position_rad,
+                    joint_count=len(mapper.joint_names),
+                )
+                return 4
+
             link.send_command(
                 CommandPacket(
                     sequence=sequence,
@@ -258,6 +382,7 @@ def main() -> int:
                 )
             )
             sequence = (sequence + 1) & 0xFFFFFFFF
+            last_hold_positions = target.astype(np.float32).copy()
 
             next_tick += mapper.control_dt
             sleep_s = next_tick - time.monotonic()
@@ -267,7 +392,7 @@ def main() -> int:
     except KeyboardInterrupt:
         print('收到 Ctrl+C，发送失能保持命令后退出。')
     finally:
-        hold = mapper.nominal_joint_targets.astype(np.float32)
+        hold = last_hold_positions.astype(np.float32)
         for _ in range(5):
             link.send_command(
                 CommandPacket(