iDP3的人形遥操代码分析：数据收集、数据转换到人形机器人VR遥控(含动作重定向)

v_JULY_v

已于 2025-04-12 16:02:28 修改

阅读量4.5k

点赞数 20

分类专栏：人形的具身大脑开发：iDP3/GR00T N1/大模型赋能文章标签： iDP3 斯坦福人形动作策略人形遥操代码 VR控制机器人遥操代码分析

于 2025-01-25 21:24:58 首次发布

本文链接：https://blog.csdn.net/v_JULY_v/article/details/145347655

版权

人形的具身大脑开发：iDP3/GR00T N1/大模型赋能专栏收录该内容

10 篇文章

订阅专栏

前言

如此文《斯坦福通用人形策略iDP3——同一套策略控制各种机器人：改进3D扩散策略，不再依赖相机校准和点云分割(含DP3的详解)》的第三部分开头所说

跑iDP3的整个流程分为：数据采集、数据转换、数据预处理，然后做训练、部署、可视化，具体而言，iDP3开源了两个代码仓库，一个是学习，一个是摇操作，其中

数据的采集，然后再进行数据转换，这都是在这种摇操作里进行的
而转换完的数据如何做可视化(当然，也可以不做可视化，即这个可视化操作是可选的)、如何训练，再如何部署是在学习的代码库里

对于后者，已经在这两篇文章里「iDP3的Learning代码解析、iDP3的训练与部署代码解析」进行了详尽细致的分析

对于前者，则是本文

第一部分 Humanoid-Teleoperation的部署与整体分析

1.1 Humanoid-Teleoperation的部署

1.1.1 硬件配置：VR、机器人及其身上的摄像头

如iDP3的遥操GitHub Humanoid-Teleoperation所述，使用 Apple Vision Pro 的人形远程操作的硬件要求：

苹果 Vision Pro
人形机器人
使用Fourier GR1作为机器人平台，以Inspire Hands作为末端执行器
RealSense 摄像头，将机器人视觉传输回 Apple Vision Pro，该项目使用了RealSense L515

这个 repo 包含两部分的代码：

Vision Pro APP安装在Apple Vision Pro上
遥控代码安装在 Fourier GR1 机载计算机上

收集训练数据并转换为训练格式以训练iDP3

关于数据，如此文《斯坦福通用人形策略iDP3——同一套策略控制各种机器人：改进3D扩散策略，不再依赖相机校准和点云分割》所说

作者后来于24年11月4日开源了他们收集的完整数据和对应的checkpoint（全部 3 个任务）：drive.google.com/drive/folders/1f5Ln_d14OQ5eSjPDGnD7T4KQpacMhgCB
而iDP3的训练数据示例training_data_example.zip见：training_data_example.zip
相当于原始数据示例存储在 Google Drive 中
通过数据转换流程转换之后便可得到iDP3格式的训练数据(训练数据示例也存储在 Google Drive 中)

1.1.2 摇操的部署

在机器人的终端中，启动机器人服务器
```
 cd humanoid_teleoperation
 bash start_robo.sh
```
在机器人的另一个终端中，启动远程操作
```
 cd humanoid_teleoperation
 bash collect_data.sh
```
在 Apple Visiob Pro 中，打开应用程序并输入
You should see the vision stream now. Snap your left hand to start the teleop.
To end the teleop, snap your left hand again.
数据默认保存到scripts/demo_dir

使用他们的脚本将数据转换为训练格式
具体如何转换呢？
$\rightarrow$ 首先，先在脚本中指定数据路径
例如，可设置路径如下humanoid_teleoperation/convert_data.sh：

 # the path to our provided raw data
 demo_path=/home/ze/projects/Humanoid-Teleoperation/humanoid_teleoperation/scripts/demo_dir/raw_data_example

 # the path to save the converted data
 save_path=/home/ze/projects/Humanoid-Teleoperation/humanoid_teleoperation/scripts/demo_dir/training_data_example

$\rightarrow$ 然后，在终端中运行脚本：

 cd humanoid_teleoperation
 bash convert_data.sh

即可获得转换后的训练数据

1.2 Humanoid-Teleoperation代码的整体分析

1.2.1 vision_pro_app的整体结构

vision_pro_app是安装在Apple Vision Pro上的应用程序，用于捕获用户的手部动作和头部姿态，并将数据传输给机器人，主要组件包括如下两大方面

第一方面，核心应用文件

`App.swift`, `ContentView.swift` - 应用程序的主要入口点和界面
`AppModel.swift` - 应用程序的核心数据模型
`HeadTrackingComponent&System.swift` - 头部跟踪组件和系统
`RealityView.swift` - 增强现实视图组件

第二方面，avp_stream：Vision Pro与机器人之间的通信组件

`streamer.py` - 数据流处理
`isaac_env.py` - 环境配置
`grpc_msg/` - 使用gRPC通信的相关文件
- `handtracking.proto` - gRPC协议定义
- `handtracking_pb2.py`, `handtracking_pb2_grpc.py` - Python gRPC生成文件
`utils/` - 工具函数
- `grpc_utils.py` - gRPC相关工具
- `se3_utils.py` - 3D空间转换工具
- `isaac_utils.py` - ISAAC相关工具

1.2.2 遥操作代码humanoid_teleoperation

这部分代码运行在机器人的板载计算机上，用于接收Vision Pro发送的数据并控制机器人，主要组件包含

第一方面，脚本文件：

collect_data.sh - 启动数据收集脚本
`convert_data.sh` - 将收集的原始数据转换为训练格式
`start_robo.sh` - 启动机器人服务器

第二方面，scripts

collect_data.py - 主数据收集脚本，调用Silverscreen类
silverscreen_multicam.py - 管理多相机数据流和手势识别
`multi_realsense.py` - RealSense摄像头接口
`action_util.py` - 动作处理工具
`rotation_util.py` - 旋转计算工具
`convert_demos.py` - 将演示数据转换为训练格式

第三方面，teleop-zenoh：基于Zenoh通信框架的遥操作核心代码

`communication.py` - 通信接口
`retarget.py` - 实现动作重定向——将人类动作映射到机器人动作
`gesture.py` - 手势识别，包含SnapMonitor类用于识别手势触发
`filter.py` - 动作平滑过滤
`run_server.py` - 运行机器人服务器
`arm_control/` - 机器人手臂控制代码
`urdf/` - 机器人模型定义文件

1.2.3 主要调用关系

1. 数据收集流程：

collect_data.sh → collect_data.py → silverscreen_multicam.py → `multi_realsense.py`
Vision Pro将手部和头部跟踪数据通过gRPC发送给机器人
silverscreen_multicam.py中的`Silverscreen`类处理从Vision Pro接收的数据
数据通过`communication.py`中的通信接口传输

2. 动作重定向流程：

从Vision Pro接收到的手部和头部姿态数据
通过`retarget.py`中的`ArmRetarget`、`HandRetarget`和`UpperBodyRetarget`类转换为机器人动作
使用`filter.py`中的`OneEuroFilter`进行平滑处理
最终通过机器人的控制接口执行动作

3. 数据转换流程

`convert_data.sh` → `convert_demos.py`
将收集的原始数据转换为训练格式，用于iDP3的训练

4. 机器人控制流程：

- `start_robo.sh` → `run_server.py`
- 启动机器人服务器，准备接收控制命令
- 接收到的命令通过机器人SDK（在`libs/`文件夹中的whl文件）控制机器人

5. Vision Pro应用流程：

用户打开Vision Pro应用
应用捕获用户的手部和头部动作
通过gRPC将数据流传输给机器人
同时接收机器人的摄像头数据流，在Vision Pro上显示

第二部分 humanoid_teleoperation/scripts代码分析

该代码库分为以下代码文件

2.1 humanoid_teleoperation/scripts

2.1.1 scripts/action_util.py

这段代码定义了一些用于处理机器人关节和末端执行器（EEF）动作的实用函数

首先，导入了必要的库，包括numpy、rotation_util和torch，然后定义了一些初始的手臂位置和四元数，以及一个14维的初始关节数组
joint32_to_joint25函数用于将32维的关节数组转换为25维的关节数组
32维的关节数组包括腰部、头部、手臂和手部的关节，而25维的关节数组只包括使用的关节。函数通过选择和重新排列32维数组中的特定元素来创建新的25维数组
joint25_to_joint32函数则是将25维的关节数组转换回32维的关节数组
它通过将25维数组中的元素放回到32维数组的相应位置来实现这一点
extract_eef_action函数用于从EEF动作数组中提取身体动作、手臂位置、手臂旋转和手部动作。它将EEF动作数组分割成不同的部分，并将手臂位置和旋转重新整形为2x3和2x6的数组
extract_abs_eef函数用于计算新的绝对位置和旋转
它接受位置增量、旋转增量、绝对位置和绝对四元数作为输入，计算新的绝对位置和旋转。函数首先将绝对四元数转换为6D旋转表示，然后将旋转增量添加到绝对旋转中，最后将新的6D旋转表示转换回四元数

这些函数在处理机器人关节和末端执行器动作时非常有用，特别是在需要在不同表示之间进行转换时

2.1.2 scripts/collect_data.py：初始化多摄像头管理对象——用于数据采集任务

这段代码初始化了一个多摄像头管理对象，并启动了一个异步主循环，用于处理数据采集任务

import asyncio          # 导入异步IO模块

import numpy as np      # 导入NumPy库

from silverscreen_multicam import Silverscreen  # 从silverscreen_multicam模块导入Silverscreen类
from termcolor import cprint  # 从termcolor模块导入cprint函数

async def ainput(prompt: str) -> str:
    # 定义一个异步函数，用于异步输入
    return await asyncio.to_thread(input, prompt)  # 使用asyncio.to_thread将input函数转换为异步函数

async def main(sscr):
    # 定义主异步函数，接受一个Silverscreen对象作为参数
    obs_fps = 50  # 设置观察帧率为50

    # await ainput("enter")  # 等待用户输入（此行被注释掉）

    cprint("Start main loop", "green")  # 打印"Start main loop"的绿色文本

    while True:
        # 进入无限循环
        await asyncio.sleep(1 / obs_fps)  # 每次循环等待1/obs_fps秒，以控制循环频率

if __name__ == "__main__":
    # 检查是否直接运行该脚本
    sscr = Silverscreen(fps=50, use_cert=True)  # 创建一个Silverscreen对象，设置帧率为50，并启用证书认证

    sscr.app.run()  # 启动Silverscreen应用程序

2.1.3 scripts/convert_demos.py：将演示数据集转换并保存为Zarr格式

humanoid_teleoperation/convert_data.sh中调用convert_demos.py

# bash convert_data.sh


save_img=1
save_depth=0


demo_path=/home/ze/projects/Humanoid-Teleoperation/humanoid_teleoperation/scripts/demo_dir/raw_data_example
save_path=/home/ze/projects/Humanoid-Teleoperation/humanoid_teleoperation/scripts/demo_dir/training_data_example

cd scripts
python convert_demos.py --demo_dir ${demo_path} \
                                --save_dir ${save_path} \
                                --save_img ${save_img} \
                                --save_depth ${save_depth} \

而convert_demos.py中实现了convert_dataset函数

总之，convert_dataset函数用于将演示数据集转换并保存为Zarr格式，而转换成Zarr格式的好处在于

高效存储与访问：Zarr专为高效存储和访问N维数组设计，特别适合大规模机器学习数据集
数据分块：代码中定义了不同数据类型的分块大小（`chunk_size`），使得可以高效地部分读取数据，训练大型数据集时尤为重要
压缩效率：使用了Blosc压缩（`compressor = zarr.Blosc(cname='zstd', clevel=3, shuffle=1)`），在保持快速读取性能的同时提供良好的压缩率
层次化组织：支持类似HDF5的层次结构（`zarr_data`和`zarr_meta`组），更易于导航和访问特定数据

至于转换的具体步骤如下

首先，它从args参数中获取演示数据目录demo_dir和保存目录save_dir，以及是否保存图像和深度数据的标志save_img和save_depth

def convert_dataset(args):
    demo_dir = args.demo_dir      # 获取演示数据目录
    save_dir = args.save_dir      # 获取保存目录
    
    save_img = args.save_img      # 获取是否保存图像的标志
    save_depth = args.save_depth  # 获取是否保存深度数据的标志

如果保存目录已经存在，函数会提示用户是否覆盖现有数据，并根据用户输入决定是否删除现有目录

    # 创建保存演示数据的目录
    if os.path.exists(save_dir):  # 如果保存目录已经存在
        # 打印数据已存在的提示信息
        cprint('Data already exists at {}'.format(save_dir), 'red')  

        # 提示用户删除现有目录
        cprint("If you want to overwrite, delete the existing directory first.", "red")  

        # 提示用户是否覆盖
        cprint("Do you want to overwrite? (y/n)", "red")          
        # user_input = input()  

        # 设置用户输入为'y'（覆盖）
        user_input = 'y'  
        # 如果用户选择覆盖
        if user_input == 'y':  
            # 打印覆盖提示信息
            cprint('Overwriting {}'.format(save_dir), 'red')      

            # 删除现有目录
            os.system('rm -rf {}'.format(save_dir))       
        # 如果用户选择不覆盖
        else:  
            # 打印退出提示信息
            cprint('Exiting', 'red')                      
            return 

    # 创建保存目录，如果目录已存在则不报错
    os.makedirs(save_dir, exist_ok=True)

接下来，函数创建保存目录，并列出演示数据目录中所有以`.h5`结尾的文件

    # 获取演示数据目录中所有以.h5结尾的文件
    demo_files = [f for f in os.listdir(demo_dir) if f.endswith(".h5")] 

    # 对文件进行排序
    demo_files = sorted(demo_files)

然后，它初始化了一些数组，用于存储颜色、深度、点云、状态、动作和结束标志的数据

    total_count = 0               # 初始化总计数
    color_arrays = []             # 初始化颜色数组
    depth_arrays = []             # 初始化深度数组
    cloud_arrays = []             # 初始化点云数组
    state_arrays = []             # 初始化状态数组
    action_arrays = []            # 初始化动作数组
    episode_ends_arrays = []      # 初始化结束标志数组

函数遍历每个演示文件，读取其中的数据，并根据需要保存颜色和深度数据

    # 遍历每个演示文件
    for demo_file in demo_files:  
        # 加载文件（h5）
        file_name = os.path.join(demo_dir, demo_file)   # 获取文件路径

        # 打印处理文件的提示信息
        print("process:", file_name)  

        with h5py.File(file_name, "r") as data:         # 打开HDF5文件
            # 如果需要保存图像
            if save_img:  
                color_array = data["color"][:]          # 读取颜色数据
              
            # 如果需要保存深度数据
            if save_depth:  
                depth_array = data["depth"][:]          # 读取深度数据
              
            cloud_array = data["cloud"][:]              # 读取点云数据
            action_array = data["action"][:]            # 读取动作数据

            # 读取本体感知数据
            proprioception_array = data["env_qpos_proprioception"][:]  
                
            # 转换为列表
            length = len(cloud_array)                   # 获取点云数据的长度

            # 如果需要保存图像
            if save_img:  
                # 将颜色数据转换为列表
                color_array = [color_array[i] for i in range(length)]  

            # 如果需要保存深度数据              
            if save_depth:  
                # 将深度数据转换为列表
                depth_array = [depth_array[i] for i in range(length)]

对于点云数据，如果点的数量超过10000，则随机采样10000个点。所有数据都被转换为列表形式，并添加到相应的数组中

            # 初始化新的点云数组
            new_cloud_array = []  

            # 遍历每个点云数据
            for i in range(length):  
                old_cloud = cloud_array[i]          # 获取旧的点云数据

                # 如果点的数量超过10000
                if old_cloud.shape[0] > 10000:      

                    # 随机采样点，随机选择10000个点
                    selected_idx = np.random.choice(old_cloud.shape[0], 10000, replace=True) 
                    new_cloud = old_cloud[selected_idx]   # 获取新的点云数据

                # 如果点的数量不超过10000
                else:  
                    new_cloud = old_cloud                 # 保持原样
                
                new_cloud_array.append(new_cloud)         # 添加新的点云数据到数组
            
            cloud_array = new_cloud_array                 # 更新点云数组
         
             # 将本体感知数据转换为列表
            proprioception_array = [proprioception_array[i] for i in range(length)] 

            # 将动作数据转换为列表
            action_array = [action_array[i] for i in range(length)]  
         
        total_count += len(action_array)                  # 更新总计数
        cloud_arrays.extend(cloud_array)                  # 扩展点云数组
       
        # 如果需要保存图像
        if save_img:  
            color_arrays.extend(color_array)              # 扩展颜色数组
  
        # 如果需要保存深度数据
        if save_depth:  
            depth_arrays.extend(depth_array)              # 扩展深度数组
            
        state_arrays.extend(proprioception_array)         # 扩展状态数组
        action_arrays.extend(action_array)                # 扩展动作数组
        episode_ends_arrays.append(total_count)           # 添加结束标志到数组

在处理完所有演示文件后，函数将这些数组转换为NumPy数组，并使用Zarr库将它们保存到指定的目录中

    # 创建Zarr文件
    zarr_root = zarr.group(save_dir)                    # 创建Zarr根组
    zarr_data = zarr_root.create_group('data')          # 创建数据组
    zarr_meta = zarr_root.create_group('meta')          # 创建元数据组

    # 将图像、状态、动作数组保存到数据组，将结束标志数组保存到元数据组
    # 如果需要保存图像
    if save_img:  
        color_arrays = np.stack(color_arrays, axis=0)              # 将颜色数组堆叠成NumPy数组
        if color_arrays.shape[1] == 3:  # 如果通道在第一维
            color_arrays = np.transpose(color_arrays, (0,2,3,1))  # 将通道移到最后一维
       
    # 如果需要保存深度数据
    if save_depth:  
        depth_arrays = np.stack(depth_arrays, axis=0)    # 将深度数组堆叠成NumPy数组
        
    state_arrays = np.stack(state_arrays, axis=0)        # 将状态数组堆叠成NumPy数组
    cloud_arrays = np.stack(cloud_arrays, axis=0)        # 将点云数组堆叠成NumPy数组
    action_arrays = np.stack(action_arrays, axis=0)      # 将动作数组堆叠成NumPy数组
    episode_ends_arrays = np.array(episode_ends_arrays)  # 将结束标志数组转换为NumPy数组

保存时，函数使用Blosc压缩器进行压缩，并根据数据类型和大小设置合适的块大小

    # 创建Blosc压缩器
    compressor = zarr.Blosc(cname='zstd', clevel=3, shuffle=1)  
    
    # 设置单个块的大小
    single_size = 500  

    # 设置状态数据的块大小
    state_chunk_size = (single_size, state_arrays.shape[1])  

    # 设置点云数据的块大小
    point_cloud_chunk_size = (single_size, cloud_arrays.shape[1], cloud_arrays.shape[2]) 

    # 设置动作数据的块大小
    action_chunk_size = (single_size, action_arrays.shape[1])  

    # 如果需要保存图像
    if save_img:  
        img_chunk_size = (single_size, color_arrays.shape[1], color_arrays.shape[2], color_arrays.shape[3])          # 设置图像数据的块大小
        zarr_data.create_dataset('img', data=color_arrays, chunks=img_chunk_size, dtype='uint8', overwrite=True, compressor=compressor)  # 创建图像数据集并保存数据
        
    # 如果需要保存深度数据
    if save_depth:  
        depth_chunk_size = (single_size, depth_arrays.shape[1], depth_arrays.shape[2])                                 # 设置深度数据的块大小
        zarr_data.create_dataset('depth', data=depth_arrays, chunks=depth_chunk_size, dtype='float32', overwrite=True, compressor=compressor)          # 创建深度数据集并保存数据

    # 创建状态数据集并保存数据
    zarr_data.create_dataset('state', data=state_arrays, chunks=state_chunk_size, dtype='float32', overwrite=True, compressor=compressor)  

    # 创建点云数据集并保存数据
    zarr_data.create_dataset('point_cloud', data=cloud_arrays, chunks=point_cloud_chunk_size, dtype='float32', overwrite=True, compressor=compressor)  

    # 创建动作数据集并保存数据
    zarr_data.create_dataset('action', data=action_arrays, chunks=action_chunk_size, dtype='float32', overwrite=True, compressor=compressor)  

    # 创建结束标志数据集并保存数据
    zarr_meta.create_dataset('episode_ends', data=episode_ends_arrays, dtype='int64', overwrite=True, compressor=compressor)

最后，函数打印保存的数据形状和范围

    # 打印数据形状
     # 如果需要保存图像
    if save_img:         
        cprint(f'color shape: {color_arrays.shape}, range: [{np.min(color_arrays)}, {np.max(color_arrays)}]', 'green')          # 打印颜色数据的形状和范围

    # 如果需要保存深度数据
    if save_depth:  
        cprint(f'depth shape: {depth_arrays.shape}, range: [{np.min(depth_arrays)}, {np.max(depth_arrays)}]', 'green')          # 打印深度数据的形状和范围
    cprint(f'cloud shape: {cloud_arrays.shape}, range: [{np.min(cloud_arrays)}, {np.max(cloud_arrays)}]', 'green')          # 打印点云数据的形状和范围
    cprint(f'state shape: {state_arrays.shape}, range: [{np.min(state_arrays)}, {np.max(state_arrays)}]', 'green')          # 打印状态数据的形状和范围
    cprint(f'action shape: {action_arrays.shape}, range: [{np.min(action_arrays)}, {np.max(action_arrays)}]', 'green')          # 打印动作数据的形状和范围
    cprint(f'Saved zarr file to {save_dir}', 'green')      # 打印保存Zarr文件的提示信息

并计算保存目录的总大小，打印出来供用户参考

    # 计算文件大小
    total_size = 0                  # 初始化总大小
    for root, dirs, files in os.walk(save_dir):          # 遍历保存目录中的所有文件
        for file in files:          # 遍历每个文件
            total_size += os.path.getsize(os.path.join(root, file))  # 累加文件大小
    cprint(f"Total size: {total_size/1e6} MB", "green")  # 打印总大小

// 待更

2.1.4 scripts/multi_realsense.py：初始化和管理多个RealSense相机，并从中获取图像和点云数据

这段代码主要用于初始化和管理多个RealSense相机，并从中获取图像和点云数据

在整个项目架构中，该模块位于视觉感知系统的底层，它：

被silverscreen_multicam.py中的`Silverscreen`类调用，为机器人提供视觉输入
处理的视觉数据会被发送到Apple Vision Pro，让操作者能看到机器人的视角
采集的视觉数据也会被用于数据收集过程中，保存为训练数据集

确保机器人能够"看到"周围环境，操作者也能通过Vision Pro看到机器人视角，从而实现有效的遥操作控制

具体而言

首先，get_realsense_id函数用于获取所有连接的RealSense设备的序列号。它创建一个RealSense上下文，查询所有设备，并获取每个设备的序列号
然后，它对这些序列号进行排序并打印出来，最后返回这些序列号
接下来，定义了三个函数init_given_realsense_L515、init_given_realsense_D455和init_given_realsense_D435，分别用于初始化不同型号的RealSense相机：L515、D455和D435

这些函数接受设备序列号和一些配置参数，创建并配置相机的管道和配置对象，启用深度和RGB流，并根据需要设置深度传感器的选项。它们还获取相机的内参信息，并返回相机的管道、对齐对象、深度比例和相机信息
grid_sample_pcd函数用于对点云进行网格采样。它接受一个点云数组和网格大小作为参数，将点云坐标缩放到网格大小，并计算每个点的网格坐标。然后，它创建唯一的网格键，并选择唯一的点，最后返回采样后的点云
CameraInfo类用于存储相机的内参信息，包括图像宽度、高度、焦距和主点坐标
SingleVisionProcess类继承自Process类，用于管理单个相机的图像和点云采集。它在初始化时接受设备序列号、队列和一些配置参数，创建相应的属性
get_vision方法用于从相机获取图像和点云数据，并进行必要的处理和裁剪
run方法用于启动相机的管道，并在循环中不断获取图像和点云数据，将其放入队列中
terminate方法用于停止相机的管道create_colored_point_cloud方法用于创建带颜色的点云

MultiRealSense类用于管理多个RealSense相机。它在初始化时获取所有连接的设备序列号，并根据配置参数创建和启动前置和右侧相机的进程

__call__方法用于从队列中获取图像和点云数据，并将其存储在字典中返回finalize方法用于终止所有相机的进程
__del__方法在对象销毁时调用finalize方法
在主程序中，创建了一个MultiRealSense对象，并在循环中不断获取图像和点云数据，保存图像并可视化点云数据

2.1.5 scripts/rotation_util.py：处理旋转和四元数

这个代码文件主要提供了一些用于处理旋转和四元数的实用函数，具体包括以下一系列函数的实现

yamlToDict函数用于将YAML文件转换为Python字典。它接受一个文件路径作为参数，读取该文件并使用

def yamlToDict(file_path):
    # 将YAML文件转换为字典
    with open(os.path.join(os.getcwd(), file_path), 'r') as f:
        cfg_dict = yaml.load(f, Loader=yaml.SafeLoader)
    return cfg_dict

最后的yaml.SafeLoader将其内容加载为字典，然后返回该字典

quatToEuler函数将四元数转换为欧拉角
它接受一个四元数数组作为输入，并计算对应的欧拉角

def quatToEuler(quat):
    # 将四元数转换为欧拉角
    eulerVec = np.zeros(3)  # 初始化欧拉角数组

首先，它提取四元数的各个分量

    qw = quat[0], qx = quat[1], qy = quat[2], qz = quat[3]  # 提取四元数分量

然后依次计算
roll（绕x轴旋转）

    # 计算roll（绕x轴旋转）
    sinr_cosp = 2 * (qw * qx + qy * qz)
    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
    eulerVec[0] = np.atan2(sinr_cosp, cosr_cosp)

pitch（绕y轴旋转

    # 计算pitch（绕y轴旋转）
    sinp = 2 * (qw * qy - qz * qx)
    if np.abs(sinp) >= 1:
        eulerVec[1] = np.copysign(np.pi / 2, sinp)  # 如果超出范围，使用90度
    else:
        eulerVec[1] = np.asin(sinp)

yaw（绕z轴旋转）

    # 计算yaw（绕z轴旋转）
    siny_cosp = 2 * (qw * qz + qx * qy)
    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
    eulerVec[2] = np.atan2(siny_cosp, cosy_cosp)

最后返回包含这三个值的数组

    return eulerVec

eulerToQuat函数将欧拉角转换为四元数
它接受一个包含roll、pitch和yaw的数组作为输入

def eulerToQuat(euler):
    # 将欧拉角转换为四元数
    roll, pitch, yaw = euler  # 提取欧拉角分量
    cy = np.cos(yaw * 0.5)
    sy = np.sin(yaw * 0.5)
    cp = np.cos(pitch * 0.5)
    sp = np.sin(pitch * 0.5)
    cr = np.cos(roll * 0.5)
    sr = np.sin(roll * 0.5)

计算对应的四元数分量

    # 计算四元数分量
    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy

并返回包含这些分量的数组

    return np.array([x, y, z, w])

quaternion_multiply函数用于计算两个四元数的乘积
它接受两个四元数数组作为输入

def quaternion_multiply(q1, q2):
    """
    计算两个四元数的乘积。
    参数:
    q1, q2 -- 表示四元数的NumPy数组 [x, y, z, w]
    """

分别提取它们的分量

    x1, y1, z1, w1 = q1      # 提取第一个四元数的分量
    x2, y2, z2, w2 = q2      # 提取第二个四元数的分量

然后根据四元数乘法公式计算结果

    # 计算乘积的分量
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2

并返回包含结果分量的数组

    return np.array([x, y, z, w])

apply_quaternion_to_vector函数将四元数表示的旋转应用于一个向量
它接受一个四元数和一个三维向量作为输入，将向量转换为四元数形式
然后计算旋转后的向量，并返回结果的前三个分量
configure_serial函数用于配置串口通信。它接受端口号和波特率作为参数，创建并配置一个serial.Serial对象，然后返回该对象
quat_rotate_inverse_np函数计算四元数的逆旋转
它接受一个四元数和一个向量作为输入，提取四元数的标量部分和向量部分，然后计算旋转后的向量，并返回结果。
quaternion_to_matrix函数将四元数转换为旋转矩阵
它接受一个包含四元数的张量作为输入，计算对应的旋转矩阵，并返回该矩阵
standardize_quaternion函数将四元数转换为标准形式，即实部为非负的形式
它接受一个包含四元数的张量作为输入，检查实部是否为负，如果是，则将四元数取反，然后返回标准化后的四元数。
_sqrt_positive_part函数计算张量中非负部分的平方根
它接受一个张量作为输入，计算每个元素的平方根，并返回结果
matrix_to_quaternion函数将旋转矩阵转换为四元数
它接受一个包含旋转矩阵的张量作为输入，计算对应的四元数，并返回该四元数
rotation_6d_to_matrix函数将6D旋转表示转换为旋转矩阵
它接受一个包含6D旋转表示的张量作为输入，使用Gram-Schmidt正交化方法计算对应的旋转矩阵，并返回该矩阵
matrix_to_rotation_6d函数将旋转矩阵转换为6D旋转表示
它接受一个包含旋转矩阵的张量作为输入，丢弃最后一行，计算对应的6D旋转表示，并返回该表示
quaternion_to_rotation_6d函数将四元数转换为6D旋转表示
它接受一个包含四元数的张量作为输入，先将四元数转换为旋转矩阵，然后将旋转矩阵转换为6D旋转表示，并返回该表示
rotation_6d_to_quaternion函数将6D旋转表示转换为四元数
它接受一个包含6D旋转表示的张量作为输入，先将6D旋转表示转换为旋转矩阵，然后将旋转矩阵转换为四元数，并返回该四元数

2.1.6 scripts/silverscreen_multicam.py

2.1.7 scripts/stream.py

// 待更

2.2 两个sh脚本：collect_data.sh、convert_data.sh

2.2.1 collect_data.sh

humanoid_teleoperation/collect_data.sh中调用collect_data.py

cd scripts

python collect_data.py

2.2.2 convert_data.sh：调用convert_demos.py

如上文「2.1.3 scripts/convert_demos.py：将演示数据集转换并保存为Zarr格式」提到过的

# bash convert_data.sh


save_img=1
save_depth=0


demo_path=/home/ze/projects/Humanoid-Teleoperation/humanoid_teleoperation/scripts/demo_dir/raw_data_example
save_path=/home/ze/projects/Humanoid-Teleoperation/humanoid_teleoperation/scripts/demo_dir/training_data_example

cd scripts
python convert_demos.py --demo_dir ${demo_path} \
                                --save_dir ${save_path} \
                                --save_img ${save_img} \
                                --save_depth ${save_depth} \

第三部分 humanoid_teleoperation/teleop-zenoh解析

3.1 teleop-zenoh/communication.py：与机器人的上半身、手部及VR设备通信

3.2.1 UDPServer类

UDPServer类用于创建一个UDP服务器，该服务器可以发送和接收UDP数据包

在初始化方法__init__中，设置了服务器的IP地址和端口，并创建了一个UDP套接字，同时设置了超时时间为0.1秒

class UDPServer:
    def __init__(self, server_ip, server_port):
        # 设置UDP服务器地址和端口
        self.server = (server_ip, server_port)

        # 创建一个UDP套接字
        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

        # 设置套接字超时时间为0.1秒
        self.socket.settimeout(0.1)

send方法将控制命令转换为JSON格式的字节流并发送到服务器

    def send(self, control_cmd):
        # 将控制命令转换为JSON格式的字节流并发送到服务器
        self.socket.sendto(
            bytes(json.dumps(control_cmd), "utf-8"), self.server)

而send_raw方法则直接发送字节流数据

    def send_raw(self, control_cmd: bytes):
        # 直接发送字节流数据到服务器
        self.socket.sendto(
            control_cmd, self.server)

3.2.2 UpperBodyCommunication类：与机器人的上半身进行通信

UpperBodyCommunication类用于与机器人的上半身进行通信

在初始化方法__init__中，创建了一个RobotClient实例，并设置了机器人的增益参数

class UpperBodyCommunication:
    def __init__(self, freq=120):
        # 创建一个RobotClient实例，设置频率为120
        self.client = RobotClient(freq)

        # 等待0.5秒
        time.sleep(0.5)

        # 启用客户端
        self.client.set_enable(True)

        # 再次等待0.5秒
        time.sleep(0.5)

        # 设置增益参数
        self.set_gains()

set_gains方法定义了机器人的比例增益（kps）

    def set_gains(self):
        # 定义比例增益参数数组
        kps = np.array([
            # 左腿
            0.875, 0.426, 0.875, 0.875, 0.416, 0.416,
            # 右腿
            0.875, 0.426, 0.875, 0.875, 0.416, 0.416,
            # 腰部
            0.25, 0.25, 0.25,
            # 头部
            0.25, 0.25, 0.2,
            # 左臂
            0.2, 0.2, 0.2, 0.2, 0.2, 0.35, 0.35,
            # 右臂
            0.2, 0.2, 0.2, 0.2, 0.2, 0.35, 0.35,
        ])

和微分增益（kds）

        # 定义微分增益参数数组
        kds = np.array([
            # 左腿
            0.023, 0.017, 0.365, 0.365, 0.007, 0.007,
            # 右腿
            0.023, 0.017, 0.365, 0.365, 0.007, 0.007,
            # 腰部
            0.14, 0.14, 0.14,
            # 头部
            0.04, 0.04, 0.005,
            # 左臂
            0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02,
            # 右臂
            0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02,
        ])

并将这些增益参数传递给机器人客户端

        # 设置客户端的增益参数
        self.client.set_gains(kps, kds)

init_set_pos方法用于初始化设置机器人的关节位置

    def init_set_pos(self, q_arm):
        # 确保传入的关节数组长度为14
        assert q_arm.shape[0] == 14

        # 初始化下半身和腰部头部的关节位置为0
        lower_body = [0.0] * 12
        waist_head = [0.0] * 6

        # 将腰部头部和手臂的关节位置合并
        joint_target_position =  waist_head + q_arm.tolist()

        # 获取当前关节位置
        joint_measured_position = self.get_pos()

        # 设定轨迹长度为20
        traj_len = 20

通过插值生成一个平滑的过渡轨迹，并逐步设置关节位置

        # 生成平滑的过渡轨迹
        for i in range(traj_len):
            cur_target = joint_measured_position * (1- i / traj_len) + np.array(joint_target_position) * (i / traj_len)

            # 设置当前目标位置
            self.set_pos(cur_target)

            # 等待0.05秒
            time.sleep(0.05)

set_pos方法将关节位置转换为度数并发布到机器人客户端

    def set_pos(self, q_total):
        # 确保传入的关节数组长度为20
        assert q_total.shape[0] == 20

        # 将关节位置转换为度数
        q_total = np.degrees(q_total)

        # 初始化下半身的关节位置为0
        lower_body = [0.0] * 12

        # 将下半身和总的关节位置合并
        joint_target_position = lower_body + q_total.tolist()

        # 发布关节位置到客户端
        self.client._publish("joint", joint_target_position)

get_pos方法获取机器人的当前关节位置并转换为弧度

    def get_pos(self):
        # 获取当前关节位置的副本
        joint_measured_position = self.client.joint_positions[12:32].copy()

        # 将关节位置转换为弧度
        joint_measured_position = np.radians(joint_measured_position)
        return joint_measured_position

3.2.3 VRCommunication类：与VR设备进行通信

VRCommunication类用于与虚拟现实设备进行通信

在初始化方法__init__中，设置了虚拟现实设备的IP地址，并创建了一个VisionProStreamer实例

class VRCommunication:
    def __init__(self, record=False, latency=0):
        # 设置虚拟现实设备的IP地址
        AVP_IP = "192.168.31.27"

        # 打印初始化信息
        print(f"Vision Pro init {AVP_IP}")

        # 保存IP地址
        self.avp_ip = AVP_IP

        # 创建一个VisionProStreamer实例
        self.s = VisionProStreamer(ip=self.avp_ip, record=record)

        # 设置延迟参数
        self.latency = latency
        
        # 如果不记录且延迟大于0，抛出未实现错误
        if record == False and latency > 0:
            raise NotImplementedError

get_data方法根据延迟参数获取最新的虚拟现实数据或记录中的数据

    def get_data(self):
        # 如果延迟为0，返回最新的数据
        if self.latency == 0:
            return self.s.latest
        else:
            # 如果记录的长度小于延迟，返回第一个记录
            if len(self.s.recording) < self.latency:
                return self.s.recording[0]
            else:
                # 否则返回延迟位置的数据
                return self.s.recording[-(self.latency+1)]

3.2.4 HandCommunication类：与机器人的手部进行通信

HandCommunication类用于与机器人的手部进行通信

在初始化方法__init__中，设置了左右手服务器的IP地址和端口

class HandCommunication:
    
    def __init__(self, stupid=False):
        # 根据stupid参数设置左右手服务器的IP地址
        if stupid:
            self._left_hand_server_ip = '192.168.137.39'
            self._right_hand_server_ip = '192.168.137.19'
        else:
            self._left_hand_server_ip = '192.168.137.19'
            self._right_hand_server_ip = '192.168.137.39'
        # 设置服务器端口
        self._server_port = 2333

并创建了两个UDPServer实例

        # 创建左右手的UDP服务器实例
        self.left_hand_udp_server = UDPServer(
            self._left_hand_server_ip, self._server_port)
        self.right_hand_udp_server = UDPServer(
            self._right_hand_server_ip, self._server_port)

        # 保存服务器实例到字典
        self.servers = {
            'left': self.left_hand_udp_server,
            'right': self.right_hand_udp_server
        }

_angle_set方法用于生成控制手部角度的命令数据包，并计算校验和

    def _angle_set(self, id, angles):
        # 创建一个字节数组
        send_data = bytearray()

        # 添加包头
        send_data.append(0xEB)  # 包头
        send_data.append(0x90)  # 包头

        # 添加灵巧手ID号
        send_data.append(id)    # 灵巧手 ID 号

        # 添加数据部分长度
        send_data.append(0x0F)  # 该帧数据部分长度 12 + 3
        # 添加写寄存器命令标志
        send_data.append(0x12)  # 写寄存器命令标志
        # 添加寄存器起始地址
        send_data.append(0xCE)  # 寄存器起始地址低八位
        send_data.append(0x05)  # 寄存器起始地址高八位

        # 以小端序添加角度值
        for angle in angles:
            angle = int(angle)
            send_data.append(angle & 0xFF)
            send_data.append((angle >> 8) & 0xFF)

        # 计算校验和
        checksum = sum(send_data[2:19]) & 0xFF
        send_data.append(checksum)

        return send_data

get_angle方法发送读取手部角度的命令，并接收和解析返回的数据包

    def get_angle(self, side: str, id: int):
        # 创建一个字节数组
        send_data = bytearray()
        # 添加包头
        send_data.append(0xEB)  # 包头
        send_data.append(0x90)  # 包头
        # 添加灵巧手ID号
        send_data.append(int(id))
        # 添加数据部分长度
        send_data.append(0x04)
        # 添加读取命令标志
        send_data.append(0x11)  # kCmd_Handg3_Read
        # 添加寄存器地址
        send_data.append(0x0a)
        send_data.append(0x06)
        send_data.append(0x0c)

        # 计算校验和
        checksum = sum(send_data[2:8]) & 0xFF
        send_data.append(checksum)

        # 获取对应的服务器实例
        server = self.servers[side]

        # 发送读取命令
        server.send_raw(send_data)
        try:
            # 接收数据
            data, addr = server.socket.recvfrom(1024)
            # 获取接收到的校验和
            received_checksum = data[19]
            # 计算校验和
            calculated_checksum = sum(data[2:19]) & 0xFF

            # 校验和不匹配则抛出错误
            if received_checksum != calculated_checksum:
                raise ValueError("Checksum mismatch")

            # 解析关节位置
            pos = [
                data[7] | (data[8] << 8),
                data[9] | (data[10] << 8),
                data[11] | (data[12] << 8),
                data[13] | (data[14] << 8),
                data[15] | (data[16] << 8),
                data[17] | (data[18] << 8)
            ]

            return pos

        except Exception as e:
            # 打印异常信息
            print(e)
            return None

send_hand_cmd方法发送左右手的角度命令，并处理接收的响应

    def send_hand_cmd(self, left_hand_angles, right_hand_angles):
        # 复制左右手的角度数组
        left_hand_angles = left_hand_angles.copy()
        right_hand_angles = right_hand_angles.copy()

        # 将角度值放大1000倍
        left_hand_angles *= 1000.
        right_hand_angles *= 1000.

        # 设置ID号为1
        id = 1

        # 生成左手的控制命令
        left_cmd = self._angle_set(id, left_hand_angles)

        # 发送左手的控制命令
        self.left_hand_udp_server.send_raw(left_cmd)
        try:
            # 接收响应
            _, _ = self.left_hand_udp_server.socket.recvfrom(1024)
        except:
            pass

        # 生成右手的控制命令
        right_cmd = self._angle_set(id, right_hand_angles)

        # 发送右手的控制命令
        self.right_hand_udp_server.send_raw(right_cmd)
        try:
            # 接收响应
            _, _ = self.right_hand_udp_server.socket.recvfrom(1024)
        except:
            pass

3.2 teleop-zenoh/config_GR1_T2.yaml

// 待更

3.3 teleop-zenoh/retarget.py：实现动作重定向(将人类动作映射到机器人动作)

这段代码是项目中的重定向(retarget)模块，是整个人形机器人遥操作系统的核心部分。它负责将用户在Apple Vision Pro中的手部和头部动作映射到机器人的关节空间中

代码包含三个主要类：ArmRetarget、HandRetarget和UpperBodyRetarget，分别处理机器人的手臂、手指和上半身的动作重定向

3.3.1 ArmRetarget类

这个类负责将用户手臂的位置和姿态转换为机器人手臂关节的角度：

初始化函数：设置机器人关节的运动范围，定义坐标系转换矩阵（`T_robot_vr`、`T_left_arm`、`T_right_arm`），以及初始化上一次优化的关节角度
代价函数：
`_fi`：一个加权代价函数
`_position_cost`：计算目标位置与实际位置之间的误差代价
`_orientation_cost`：计算目标方向与实际方向之间的误差代价
`_joint_velocity_cost`：计算关节速度代价，防止机器人动作过于剧烈
`_objective_function`：综合上述代价的优化目标函数
求解手腕角度：`_solve_wrist_angles`方法计算手腕关节的角度，基于手腕帧和前臂帧的相对位置和姿态
求解上臂角度：`_solve_uparm_angles`方法通过优化算法（SLSQP）求解上臂的5个关节角度
整体求解：`solve_arm_angles`方法整合前面的方法，处理数据缺失情况，执行坐标变换，并最终输出14维的关节角度数组