Day12 unitree_G1人形机器人优先拿到所有真实帧

别动我齐刘海

19人浏览 · 2026-08-12 09:27:56

别动我齐刘海 · 2026-08-12 09:27:56 发布

一、今天最终修改内容

1. 修正 `PollApplicationNextEvent` 的容量参数

为什么修改：PollApplicationNextEvent 的第二个参数表示事件数组的元素数量/容量，不是内存字节数。

旧代码传入：

sizeof(MCPEvent_t)

等于告诉SDK“当前指针后面能存放很多个事件”，但实际上只分配了一个MCPEvent_t。这可能导致：

SDK一次写入多个事件；
后面的事件覆盖相邻内存；
当前事件内容被破坏；
Avatar事件被跳过；
帧读取顺序异常；
极端情况下发生内存越界。

修改依据：

Noitom提供的C#封装对该参数的使用方式是：

先传容量0；
SDK返回所需事件数量；
根据该数量创建MCPEvent_t[]；
再调用API读取事件。

这证明参数语义是“事件数量”，不是字节数。

我们现在采用逐事件处理，因此容量必须为：

event_count = 1;

每取到一个Avatar事件，就立即把对应姿态深拷贝完成，然后再取下一个事件。

2. `Error_MoreEvent` 后立即继续读取

当前逻辑：

if (error == MocapApi::Error_MoreEvent) {
    ++more_event_returns;
}

随后正常处理当前事件。处理完后，采集线程马上再次调用pollFrame()。

为什么这样处理：

Error_MoreEvent不是失败，而是表示：

本次事件有效，并且SDK队列里还有其他事件

所以不能把它当成空事件，也不能等待1毫秒。

最终行为：

读取当前事件
→ 深拷贝当前Avatar帧
→ 放入FIFO
→ 立即再次poll

这样能够尽快排空SDK事件队列，降低SDK内部姿态缓存被更新覆盖的概率。

3. 非Avatar事件立即继续Poll

SDK队列中可能出现：

Notify
SensorModulesUpdated
TrackerUpdated
AvatarUpdated

GMR只需要Avatar姿态，但无关事件不能拖慢后面的Avatar事件。

旧式流程可能是：

Notify
→ 返回外层
→ sleep 1ms
→ SensorUpdated
→ 再sleep 1ms
→ AvatarUpdated

当前流程是：

Notify
→ 立即poll
→ SensorUpdated
→ 立即poll
→ AvatarUpdated
→ 深拷贝

它不丢弃SDK队列，只是不把非Avatar事件交给GMR。

4. 真正无事件时使用 `yield()`

50Hz一帧周期是20ms。固定等待1ms看似不长，但Windows线程调度下实际唤醒时间可能超过1ms。如果SDK事件刚好在sleep_for()之后到达，就会增加采集延迟。队列繁忙时，这种固定等待可能不断累积。

yield()的含义是：

当前没有事件
→ 主动把CPU让给其他可运行线程
→ 尽快再次获得CPU并轮询SDK

注意：yield()可能导致空轮询次数达到每秒几百万次。这会提高CPU占用，但它符合当前“优先保证采集、不增加固定等待”的目标。

5. 每个Avatar事件立即深拷贝

深拷贝内容包括：

timestamp_ms
avatar_handle
posture_index
所有使用关节的位置
所有使用关节的旋转
独立的std::vector<BodyPose>

为什么必须立即深拷贝：

MocapApi的Avatar和Joint句柄指向SDK内部缓存，不是独立帧对象。

如果只保存：

AvatarHandle
JointHandle
指针
引用

等到GMR线程读取时，SDK内部缓存可能已经更新成下一帧，最终表现就会类似“只读到最新帧”。

当前方案是：

收到AvatarUpdated
→ 立即读取posture_index
→ 立即读取所有关节
→ 复制到独立NoitomFrame
→ 不再依赖SDK缓存

因此进入FIFO后的每个NoitomFrame都有自己的数据存储。

6. 深拷贝前后再次检查 `posture_index`

即使Avatar事件已经收到，SDK内部姿态也可能在读取几十个关节期间更新。

如果发生：

复制前 posture_index=100
复制上半身时还是100
复制下半身时SDK更新到101
复制后 posture_index=101

那么这一帧可能是混合帧。

新增指标：

posture_changes_during_copy

如果它始终为0，说明在当前机器和50Hz条件下，整帧深拷贝速度足够快。

7. 采集线程与GMR线程继续分离

当前线程结构保持为：

MocapApi采集线程
        ↓
NoitomFrame FIFO
        ↓
GMR线程
        ↓
RobotFrame FIFO
        ↓
发送线程

没有把MocapApi轮询放进GMR线程。

为什么保持分离：

GMR计算耗时存在波动。如果采集和GMR在同一线程：

采集一帧
→ 执行GMR
→ GMR偶尔耗时增加
→ 不能及时Poll SDK
→ SDK缓存继续更新
→ 中间Avatar帧可能被覆盖

分离以后：

采集线程只负责Poll和深拷贝
GMR线程只负责从FIFO逐帧计算

即使GMR短时间变慢，已经采集的帧仍保存在FIFO中，不会被“最新帧”替换。

8. FIFO不采用“只保留最新帧”

当前使用：

std::deque<NoitomFrame> input_queue_;

生产者：

input_queue_.push_back(frame);

消费者：

frame = std::move(input_queue_.front());
input_queue_.pop_front();

最终语义是：

先进先出
每帧入队
每帧出队
不覆盖队尾
不清空旧帧

这与机器人端26帧左右的缓冲逻辑相匹配。

如果采用“latest frame”模式：

shared_frame = newest_frame;

那么GMR一旦来不及处理，旧帧会直接被新帧覆盖。当前代码没有使用这种方案。

9. 提升MocapApi采集线程优先级

增加：

SetThreadPriority(
    GetCurrentThread(),
    THREAD_PRIORITY_HIGHEST);

为什么修改：采集线程需要尽快响应SDK事件。Axis窗口、MuJoCo渲染、GMR计算以及其他Windows后台任务都可能争用CPU。

把采集线程设置为HIGHEST可以降低：

线程被长时间抢占；
max_poll_gap_ms突然变大；
SDK事件积压；
SDK缓存更新覆盖的概率。

没有使用：

THREAD_PRIORITY_TIME_CRITICAL
REALTIME_PRIORITY_CLASS

因为实时优先级可能挤压系统、网络和SDK内部线程，反而导致不稳定。

启动成功会显示：

[MotionInput] acquisition thread priority=highest

10. 增加并默认开启缺失帧详细日志

发生单帧缺失：

[NoitomJump]
prev_posture_index=2256
posture_index=2258
delta=2
missing_posture_indices=[2257]

连续缺失两帧：

prev_posture_index=3155
posture_index=3158
delta=3
missing_posture_indices=[3156,3157]

同时输出：

loop_gap_ms
poll_block_ms
source_dt_ms
arrival_dt_ms
ptp_dt_ms
poll_error
evidence

为什么增加：

只看“共丢了20帧”不能定位每次问题。现在可以知道：

缺失的是哪个编号；
一次缺一帧还是多帧；
当时采集线程有没有被阻塞；
SDK调用有没有长时间阻塞；
源时间戳是否同时跳跃；
问题更可能发生在本机还是上游。

11. 完善采集统计

现在保留以下指标：

指标	含义
`poll_calls`	调用SDK Poll的次数
`avatar_events`	取得的Avatar事件数量
`empty_polls`	SDK无事件次数
`other_events`	非Avatar事件数量
`more_event_returns`	SDK表示队列仍有事件的次数
`duplicate_indices`	连续Avatar事件读取到相同姿态编号
`backward_indices`	姿态编号倒退次数
`skipped_frames`	当前统计周期内缺失编号数量
`posture_changes_during_copy`	深拷贝期间SDK姿态变化次数
`max_poll_gap_ms`	两次进入Poll之间的最大间隔
`max_poll_block_ms`	单次SDK Poll调用最大耗时
`avg_avatar_read_ms`	平均整帧深拷贝耗时
`max_avatar_read_ms`	最大整帧深拷贝耗时
`source_missing_indices`	整次运行累计缺失姿态编号数

其中判断“真实姿态编号是否连续”的主指标是：

posture_index
source_missing_indices
missing_posture_indices

二、明确没有做的事情

没有插值补帧

曾考虑过在posture_index跳号时使用前后姿态插值，但根据你的要求已撤回。

当前不会：

100 → 102

自动生成：

101（插值帧）

原因是现阶段首要目标是：

验证能否取得全部真实帧

如果现在就补帧，虽然机器人接收数量连续，但会掩盖采集端真实缺失，导致无法判断MocapApi调用是否已经解决。

没有丢弃重复Avatar帧

当前重复帧仍会深拷贝并进入FIFO，同时统计：

duplicate_indices

这样可以观察SDK是否出现：

多个AvatarUpdated事件
→ 实际都指向同一个最新posture_index

如果现在直接过滤重复帧，也可能掩盖SDK缓存覆盖证据。

没有修改

GMR算法；
IK配置；
MuJoCo模型；
控制算法；
ZMQ消息结构；
机器人端26帧缓冲；
机器人消费逻辑。

三、技术判断链

本次问题按以下层次分析：

第一层：机器人端为什么会累计误差

机器人按50Hz逐帧消费，缓冲区只负责暂存，不会恢复缺失帧。

例如：

应收到：100 101 102 103 104
实际收到：100 101 103 104 105

如果机器人只按队列位置消费，不检查源姿态编号，那么从缺失的102开始，队列位置和真实时间轴错开一帧。

多次缺失后，偏差会累积。

第二层：程序是否覆盖已经收到的帧

检查结果：

采集线程与GMR线程分离；
使用deque FIFO；
每帧push_back()；
GMR逐帧pop_front()；
没有“保存最新一帧”的单槽共享变量。

因此应用层FIFO没有主动覆盖旧帧。

第三层：SDK事件是否被正确逐个读取

发现关键错误：

event_size = sizeof(MCPEvent_t);

SDK参数实际是事件数量。这可能破坏事件读取和内存边界。

因此优先修复为：

event_count = 1;

第四层：SDK事件对应的数据是否立即独立保存

Avatar事件只携带Avatar句柄，关节数据来自SDK内部缓存。

因此必须：

Poll一个Avatar事件
→ 马上读取posture_index
→ 马上复制所有关节
→ 形成独立NoitomFrame

不能先把Avatar句柄存入队列，之后再读取。

第五层：本机调度是否可能来不及Poll

采用：

独立采集线程；
空事件时yield()；
非Avatar事件立即继续；
采集线程HIGHEST；
统计max_poll_gap_ms；
统计max_poll_block_ms。

第六层：如何证明是否真正解决

最终不依赖机器人动作是否“看起来正常”，而是检查：

source_missing_indices=0
skipped_frames=0
duplicate_indices=0
backward_indices=0
posture_changes_during_copy=0

并验证：

last_posture_index - first_posture_index + 1
=
source_received

在无重复、无倒退情况下，这个等式成立就说明整个测试区间姿态编号连续。

四、最终程序框架

```mermaid
flowchart LR
    A["Axis Studio<br/>50 Hz姿态源"] --> B["UDP 127.0.0.1:7012"]
    B --> C["MocapApi内部事件队列"]

    subgraph T1["高优先级MocapApi采集线程"]
        C --> D["PollApplicationNextEvent<br/>event_count = 1"]
        D --> E{"事件类型"}
        E -->|"无事件"| F["yield()"]
        F --> D
        E -->|"非Avatar事件"| D
        E -->|"AvatarUpdated"| G["立即读取posture_index"]
        G --> H["立即读取全部关节位置和旋转"]
        H --> I["深拷贝为独立NoitomFrame"]
        I --> J["检查拷贝前后posture_index"]
    end

    J --> K["NoitomFrame FIFO<br/>push_back"]

    subgraph T2["普通优先级GMR线程"]
        K --> L["pop_front"]
        L --> M["GMR/IK重定向"]
        M --> N["RobotFrame"]
    end

    N --> O["RobotFrame FIFO"]

    subgraph T3["发送线程"]
        O --> P["按配置帧率生成发送帧"]
        P --> Q["通信层"]
    end

    Q --> R["机器人约26帧Buffer"]
    R --> S["50 Hz逐帧消费"]
```

五、采集线程核心逻辑图

```mermaid
flowchart TD
    A["进入pollFrame()"] --> B["记录poll进入时间"]
    B --> C["创建一个MCPEvent"]
    C --> D["event_count = 1"]
    D --> E["PollApplicationNextEvent"]

    E --> F{"返回结果"}

    F -->|"Error_NoneMessage<br/>或MCPEvent_None"| G["empty_polls++"]
    G --> H["返回空"]
    H --> I["外层yield()"]
    I --> A

    F -->|"Error_InsufficientBuffer"| J["报告SDK容量契约错误"]
    F -->|"其他错误"| K["抛出系统错误"]

    F -->|"Error_None或Error_MoreEvent"| L{"事件是否AvatarUpdated"}

    L -->|"否"| M["other_events++"]
    M --> C

    L -->|"是"| N["avatar_events++"]
    N --> O["读取posture_index_before"]
    O --> P["检测重复/倒退/跳号"]
    P --> Q["输出missing_posture_indices"]
    Q --> R["深拷贝所有关节"]
    R --> S["读取posture_index_after"]
    S --> T{"复制期间是否变化"}

    T -->|"是"| U["posture_changes_during_copy++"]
    T -->|"否"| V["保持0"]

    U --> W["返回独立NoitomFrame"]
    V --> W
    W --> X["FIFO push_back"]
    X --> A
```