前言
本文是接上文《iDP3的Learning代码解析：逐步分解人形策略斯坦福iDP3的数据集、模型、动作预测策略代码(包含2D和3D两个版本)》而来的
如此文《斯坦福通用人形策略iDP3——同一套策略控制各种机器人：改进3D扩散策略，不再依赖相机校准和点云分割(含DP3的详解)》的开头所说，我司正在借助iDP3做通用化改写，使得一套策略控制各种机器人
而关于iDP3的介绍「如作者所说，iDP3 是适用于任何机器人的通用 3D 视觉运动策略，且可以在没有相机校准和点云分割的情况下使用 iDP3」在之前的文章都详细分析过了「详见《此文》」
故本文侧重介绍
- iDP3的预处理、训练、部署，比如iDP3 三个核心脚本包括：
vis_dataset.sh、train_policy.sh、deploy_policy.sh
分别代表了可视化、训练、部署，且他们分别作为对应 py 脚本的参数设置前置环节 - 对应的py 脚本
且如此文前言部分的最后所说，本文依然会一如既往的保持以下几点措施
- 每一段待解读的代码，尽可能控制在10行以内，因为按我的经验，超过10行 看着就累了
- 即便有解读，贴的代码 也要逐行都有对应的注释
- 对于较长的代码文件，我会特意在分析代码文件之前，贴一下对应的代码结构截图
- 给每个章节的代码文件名称都加上了对应的一句话说明
最后，本文的写就有「我司机器人方向技术合伙人姚博士」的重要贡献，且本文之外，建议进一步学习(你会得到进一步的巩固)：此课程的《第十二次课 基于iDP3的umi-dexcap通用化部署(我司独创工作：对iDP3的通用化改写)》，我自己也认真看了两遍
目录
第一部分 iDP3的数据可视化：vis_dataset.sh、vis_dataset.py
1.2 Improved-3D-Diffusion-Policy/vis_dataset.py
第二部分 iDP3的训练：train_policy.sh、train.py
2.2 Improved-3D-Diffusion-Policy/train.py：调用对应的yaml文件
2.3.2 训练流程：train_policy.py 从 idp3.yaml 中获取类，并实例化idp3_workspace
第三部分 iDP3的部署：deploy_policy.sh、deploy.py
3.2 Improved-3D-Diffusion-Policy/deploy.py：部署全流程
3.2.2 类 GR1DexEnvInference：用于机器人部署
第一部分 iDP3的数据可视化：vis_dataset.sh、vis_dataset.py
1.1 scripts/vis_dataset.sh
这个 vis_dataset.sh 脚本用于可视化3D扩散策略模型的训练数据，或者如姚博士所说vis_dataset.sh 脚本调用 vis_dataset.py 文件以可视化 2D/3D 数据集
因此不需要额外写脚本，可以直接借助开源的代码直接可视化训练数据
运行方法非常简单，首先下载开源的样例数据集 training_data_example.zip 并解压「 如此文《斯坦福通用人形策略iDP3——同一套策略控制各种机器人：改进3D扩散策略，不再依赖相机校准和点云分割(含DP3的详解)》第二部分的开头所述，iDP3的训练数据示例training_data_example.zip见：training_data_example.zip」，然后对应修改脚本中的 dataset_path 为解压后文件夹位置，并更改 --vis_cloud=1，至于vis_dataset.py 中需要对应修改的部分会在下一节中讲解
具体而言
- 脚本的开头提供了一个示例命令，展示了如何使用该脚本
# bash scripts/vis_dataset.sh - 首先，脚本定义了数据集路径 `dataset_path`——注意，凡是这种路径设置 你都要改成你本地电脑上的实际路径，指向训练数据的示例目录
dataset_path=/home/ze/projects/Improved-3D-Diffusion-Policy/training_data_example - 接着，设置了一个变量 `vis_cloud`，用于控制是否可视化点云数据
vis_cloud=0 # 是否可视化点云数据的标志，0 表示不可视化 - 然后，脚本切换到 Improved-3D-Diffusion-Policy 目录
并运行 `vis_dataset.py` 脚本——下一节将详细介绍该脚本cd Improved-3D-Diffusion-Policy # 切换到项目目录
该脚本接受多个参数，包括数据集路径、是否使用图像、是否可视化点云、是否使用点云颜色以及是否对数据进行下采样
通过以上这些参数，`vis_dataset.py` 脚本可以根据指定的配置对数据集进行可视化，帮助用户更好地理解和分析训练数据# 运行 vis_dataset.py 脚本，指定数据集路径 python vis_dataset.py --dataset_path $dataset_path \ --use_img 1 \ # 使用图像进行可视化 --vis_cloud ${vis_cloud} \ # 是否可视化点云数据 --use_pc_color 0 \ # 不使用点云颜色 --downsample 1 \ # 对数据进行下采样
最终，运行效果如下图所示(来自合伙人姚博士)
25年2.27日，我自己也实际运行了下这套数据可视化的代码，没有问题
且值得一提的是，copilot确实好用，全程在VS code专业版的copilot agent模式下：下指令就行了(配套的 Claude 3.7)，电脑没有的环境自动帮我安装，需要修改的路径、代码、命令直接帮我改..
1.2 Improved-3D-Diffusion-Policy/vis_dataset.py
这个 vis_dataset.py 脚本用于可视化3D扩散策略模型的训练数据，或者如姚博士所说，vis_dataset.py 主要作用在于点云数据的可视化，并可以做一些简单的预处理
关键参数基本都在 vis_dataset.sh 中定义了，需要改动的仅以下两点：
- 点云图像保存位置，因为 dataset_path 被设置为了绝对路径，因此需要相应修改：
save_dir = f"{dataset_path}/{episode_idx}" # 设置当前集的保存目录 - 点云视频保存位置，对应修改：
if vis_cloud: # 将图像序列转换为视频 os.system(f"ffmpeg -r 10 -i {save_dir}/%d.png -vcodec mpeg4 -y {dataset_path}/{episode_idx}.mp4")
运行后，可生成
对原代码文件我也详细介绍下，具体而言
- 脚本首先导入必要的模块，包括 os、argparse 和 numpy
import zarr # 导入 zarr 库，用于处理 zarr 格式的数据 import cv2 # 导入 OpenCV 库，用于图像处理 from termcolor import cprint # 导入 cprint 函数，用于彩色打印 import time # 导入 time 模块，用于时间相关操作 from tqdm import tqdm # 导入 tqdm 库，用于显示进度条 import visualizer # 导入自定义的 visualizer 模块 import os # 导入 os 模块，用于操作系统相关功能 import argparse # 导入 argparse 模块，用于解析命令行参数 import numpy as np # 导入 numpy 库，并重命名为 np - 然后，使用 argparse 模块解析命令行参数
定义了几个参数：dataset_path（数据集路径）、use_img（是否使用图像）、vis_cloud（是否可视化点云）、use_pc_color（是否使用点云颜色）和 downsample（是否对数据进行下采样）# 创建 ArgumentParser 对象 parser = argparse.ArgumentParser() # 添加命令行参数 parser.add_argument("--dataset_path", type=str, default="data/box_zarr") parser.add_argument("--use_img", type=int, default=0) parser.add_argument("--vis_cloud", type=int, default=0) parser.add_argument("--use_pc_color", type=int, default=0) parser.add_argument("--downsample", type=int, default=0) - 解析参数后，脚本打开指定路径的数据集，并打印数据集的结构树
# 解析命令行参数 args = parser.parse_args() use_img = args.use_img # 是否使用图像 dataset_path = args.dataset_path # 数据集路径 vis_cloud = args.vis_cloud # 是否可视化点云 use_pc_color = args.use_pc_color # 是否使用点云颜色 downsample = args.downsample # 是否对数据进行下采样 # 打开 zarr 格式的数据集 with zarr.open(dataset_path) as zf: print(zf.tree()) # 打印数据集的结构树 - 接着，根据参数 use_img 决定是否加载图像数据，并加载所有的点云数据和元数据中的每个 episode 的结束位置
本质上输入是img、point_cloud、state，输出是action
# 获取数据 if use_img: all_img = zf['data/img'] # 获取所有图像数据 all_point_cloud = zf['data/point_cloud'] # 获取所有点云数据 all_episode_ends = zf['meta/episode_ends'] # 获取所有 episode 的结束位置 - 脚本遍历每个 episode，根据 `episode_ends` 将数据分割成不同的 episode
如果是第一个 episode，则从头开始截取数据
否则，从上一个 episode 的结束位置开始截取数据# 根据 episode_ends 将数据分割成不同的 episode for episode_idx, episode_end in enumerate(all_episode_ends): if episode_idx == 0: if use_img: img_episode = all_img[:episode_end] # 获取第一个 episode 的图像数据 point_cloud_episode = all_point_cloud[:episode_end] # 获取第一个 episode 的点云数据
然后，创建保存可视化结果的目录，并打印当前正在回放的 episodeelse: if use_img: # 获取后续 episode 的图像数据 img_episode = all_img[all_episode_ends[episode_idx-1]:episode_end] # 获取后续 episode 的点云数据 point_cloud_episode = all_point_cloud[all_episode_ends[episode_idx-1]:episode_end]# 设置保存可视化结果的目录 save_dir = f"visualizations/{dataset_path}/{episode_idx}" if vis_cloud: os.makedirs(save_dir, exist_ok=True) # 创建保存目录 cprint(f"replay episode {episode_idx}", "green") # 打印当前正在回放的 episode - 在每个 episode 中，脚本遍历每一帧的数据
如果启用了下采样，则随机选择4096个点进行下采样# 回放图像 for i in range(point_cloud_episode.shape[0]): pc = point_cloud_episode[i] # 获取当前帧的点云数据
如果使用图像，则将图像转换为 RGB 格式并显示# 下采样 if downsample: num_points = 4096 # 下采样后的点数 idx = np.random.choice(pc.shape[0], num_points, replace=False) # 随机选择点 pc = pc[idx] # 获取下采样后的点云数据
对于点云数据，如果启用了点云可视化，则根据参数决定是否使用点云颜色，并将点云数据保存为图像文件if use_img: img = img_episode[i] # 获取当前帧的图像数据 img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) # 将图像转换为 RGB 格式 cv2.imshow('img', img) # 显示图像 cv2.waitKey(1) # 等待1毫秒 time.sleep(0.05) # 暂停0.05秒# if vis_cloud and i >= 50: if vis_cloud: if not use_pc_color: pc = pc[:, :3] # 如果不使用点云颜色，只保留前三列 visualizer.visualize_pointcloud(pc, img_path=f"{save_dir}/{i}.png") # 可视化点云并保存为图像文件 print(f"vis cloud saved to {save_dir}/{i}.png") # 打印保存信息 print(f"frame {i}/{point_cloud_episode.shape[0]}") # 打印当前帧的信息 - 最后，如果启用了点云可视化，脚本会将保存的图像文件转换为视频文件，并保存到指定目录中
通过这些步骤，脚本可以根据配置对数据集进行可视化，帮助用户更好地理解和分析训练数据if vis_cloud: # 转换为视频 os.system(f"ffmpeg -r 10 -i {save_dir}/%d.png -vcodec mpeg4 -y visualizations/{dataset_path}/{episode_idx}.mp4") # 将图像文件转换为视频文件
第二部分 iDP3的训练：train_policy.sh、train.py
2.1 scripts/train_policy.sh
这个 train_policy.sh 脚本用于训练一个3D扩散策略模型
- 脚本的开头提供了两个示例命令，展示了如何使用该脚本
脚本接受三个参数：算法名称 (`alg_name`)、任务名称 (`task_name`) 、附加信息 (`addition_info`)
比如下面
第一条示例命令是，基于iDP3算法的gr1和灵巧手3D任务
第二条示例命令是，基于DP的gr1和灵巧手2D图像任务# 示例命令： # bash scripts/train_policy.sh idp3 gr1_dex-3d 0913_example # bash scripts/train_policy.sh dp_224x224_r3m gr1_dex-image 0913_example - 首先，脚本定义了数据集路径 `dataset_path`，并设置了一些默认参数，如调试模式 (`DEBUG`) 和 `wandb` 模式 (`wandb_mode`)
# 数据集路径 dataset_path=/home/ze/projects/Improved-3D-Diffusion-Policy/training_data_example DEBUG=False # 调试模式 wandb_mode=offline # wandb 模式 - 接着，它从命令行参数中获取算法名称、任务名称和附加信息，并使用这些信息生成实验名称 (`exp_name`) 和运行目录 (`run_dir`)
alg_name=${1} # 从命令行参数获取算法名称 task_name=${2} # 从命令行参数获取任务名称 config_name=${alg_name} # 配置名称与算法名称相同 addition_info=${3} # 从命令行参数获取附加信息 seed=0 # 随机种子 exp_name=${task_name}-${alg_name}-${addition_info} # 生成实验名称 run_dir="data/outputs/${exp_name}_seed${seed}" # 生成运行目录 - 脚本还设置了 GPU 的 ID，并打印出使用的 GPU ID
如果调试模式 (`DEBUG`) 为真，则不保存检查点，并打印调试模式的提示信息；gpu_id=0 # GPU ID echo -e "\033[33mgpu id (to use): ${gpu_id}\033[0m" # 打印使用的 GPU ID
否则，保存检查点，并打印训练模式的提示信息if [ $DEBUG = True ]; then # 如果调试模式为真 save_ckpt=False # 不保存检查点 # wandb_mode=online # 注释掉的 wandb 在线模式 echo -e "\033[33mDebug mode!\033[0m" # 打印调试模式提示信息 echo -e "\033[33mDebug mode!\033[0m" # 打印调试模式提示信息 echo -e "\033[33mDebug mode!\033[0m" # 打印调试模式提示信息else # 如果调试模式为假 save_ckpt=True # 保存检查点 echo -e "\033[33mTrain mode\033[0m" # 打印训练模式提示信息 - 然后，脚本切换到 Improved-3D-Diffusion-Policy 目录，并设置一些环境变量，如 `HYDRA_FULL_ERROR` 和 `CUDA_VISIBLE_DEVICES`
cd Improved-3D-Diffusion-Policy # 切换到项目目录 export HYDRA_FULL_ERROR=1 # 设置环境变量，显示完整的 Hydra 错误信息 export CUDA_VISIBLE_DEVICES=${gpu_id} # 设置可见的 GPU 设备 - 最后，脚本运行 `train.py` 脚本——下一节即将介绍该脚本，并传递多个参数，具体如下所示
这些参数用于配置和运行模型的训练过程# 运行 train.py 脚本，指定配置文件 python train.py --config-name=${config_name}.yaml \ task=${task_name} \ # 设置任务名称 hydra.run.dir=${run_dir} \ # 设置运行目录 training.debug=$DEBUG \ # 设置调试模式 training.seed=${seed} \ # 设置随机种子 training.device="cuda:0" \ # 设置训练设备为 GPU exp_name=${exp_name} \ # 设置实验名称 logging.mode=${wandb_mode} \ # 设置日志模式 checkpoint.save_ckpt=${save_ckpt} \ # 设置是否保存检查点 task.dataset.zarr_path=$dataset_path # 设置数据集路径
2.2 Improved-3D-Diffusion-Policy/train.py：调用对应的yaml文件
这个 Python 脚本用于训练一个3D扩散策略模型
- 脚本首先导入必要的模块
并设置标准输出和标准错误为行缓冲模式，以便实时输出日志信息""" 用法: 训练: python train.py --config-name=train_diffusion_lowdim_workspace """ # 导入 os 模块，用于操作系统相关功能 import os # 导入 BaseWorkspace 类 from diffusion_policy_3d.workspace.base_workspace import BaseWorkspace # 导入 pathlib 模块，用于路径操作 import pathlib # 导入 OmegaConf 模块，用于配置解析 from omegaconf import OmegaConf # 导入 hydra 模块，用于配置管理 import hydra # 导入 cprint 函数，用于彩色打印 from termcolor import cprint # 导入 sys 模块，用于系统相关功能 import sys# 使用行缓冲模式设置 stdout 和 stderr sys.stdout = open(sys.stdout.fileno(), mode='w', buffering=1) sys.stderr = open(sys.stderr.fileno(), mode='w', buffering=1) - 接着，通过设置环境变量 `WANDB_SILENT` 为 `True` 来静默 `wandb` 的输出
# 设置环境变量，静默 wandb 输出 os.environ['WANDB_SILENT'] = "True" - 然后，脚本使用 OmegaConf 注册了一个新的解析器 eval，允许在配置文件中执行任意的 Python 代码
# 允许在配置中执行任意 Python 代码 OmegaConf.register_new_resolver("eval", eval, replace=True) - 接下来，脚本使用 hydra.main 装饰器定义了 main 函数，并指定了配置文件的路径
@hydra.main( config_path=str(pathlib.Path(__file__).parent.joinpath( 'diffusion_policy_3d', 'config')) # 配置文件路径
上面这段代码相当于指定配置文件的路径，配置文件存放在 diffusion_policy_3d/config 文件夹下——在此文《iDP3的Learning代码解析：逐步分解iDP3的数据集、模型、动作预测策略代码(包含2D和3D两个版本)》的「1.2 diffusion_policy_3d/config」已介绍
其配合 --config-name 参数，加载特定的配置文件
- main 函数接受一个 OmegaConf 对象 cfg 作为参数，并立即解析配置文件中的所有解析器。然后，通过 hydra.utils.get_class 获取配置中指定的类，并实例化一个 workspace 对象
def main(cfg: OmegaConf): # 立即解析配置，以便所有 ${now:} 解析器使用相同的时间 OmegaConf.resolve(cfg) cls = hydra.utils.get_class(cfg._target_) # 获取目标类 workspace: BaseWorkspace = cls(cfg) # 实例化工作空间 - 最后，调用 workspace.run() 方法运行训练过程
workspace.run() # 运行工作空间 - 如果脚本作为主程序运行，则调用 main 函数，开始训练过程
if __name__ == "__main__": main() # 调用 main 函数
2.3 管理配置库Hydra与训练流程小结
2.3.1 管理配置库Hydra：动态配置Python项目
为方便大家理解，特地再解释一下Hydra库
- 首先，iDP3 无论是 train 还是 deploy 均使用了 Hydra 这一个python 库
Hydra 是一个用于管理复杂配置的开源框架，特别适用于需要动态配置的 Python 项目。它的主要功能是允许用户通过多种方式（如配置文件、命令行参数、环境变量等）管理和合成配置
而 iDP3 中就是使用 Hydra 管理 yaml 配置文件 - Hydra 使用 YAML 语言书写配置文件。YAML是一种简洁的数据序列化格式，常用于配置文件、数据交换、日志记录等场景。通常把需要的配置写在 config.yaml 中
在运行 application 时候，config.yaml 会自动加载
也可以在 application 中通过命令行覆盖 config.yaml 中的值
2.3.2 训练流程：train_policy.py 从 idp3.yaml 中获取类，并实例化idp3_workspace
最后总结一下
- train_policy.sh 传参了 policy、task 以及 addition_info，并设定了 DEBUG 模式
bash scripts/train_policy.sh idp3 ur5e_leap 1228_example - 此后，train_policy.py 从 idp3.yaml 中获取类，并实例化工作空间 idp3_workspace.py
同时，hydra.main 主函数装饰器在运行 idp3_workspace.py 时候，config 会自动加载文件夹下的所有 yaml 配置文件，包括 task 和 idp3
相当于先 idp3.yaml 然后再 idp3_workspace.py 及 gr1_dex-3d.yaml
而idp3.yaml 设置了绝大部分关键的策略配置，全逻辑可以总结为：
- 任务定义: 训练一个扩散模型，用于点云数据的序列生成 ->
- 数据编码: 使用PointNet对点云数据进行多阶段特征提取 ->
- 扩散过程: 基于DDIMScheduler调度生成点云动作序列 ->
- 训练优化: 使用AdamW优化器，通过余弦调度策略逐步降低学习率 ->
- 结果管理: 配置检查点保存和日志记录，确保实验过程可控
更具体的idp3.yaml介绍，详见此文《iDP3的Learning代码解析：逐步分解iDP3的数据集、模型、动作预测策略代码(包含2D和3D两个版本)》的「1.2.3 config/idp3.yaml：相当于配置文件」
第三部分 iDP3的部署：deploy_policy.sh、deploy.py
3.1 scripts/deploy_policy.sh
这个 deploy_policy.sh 脚本用于部署一个3D扩散策略模型。脚本的开头提供了两个示例命令，展示了如何使用该脚本。脚本接受三个参数：算法名称 (`alg_name`)、任务名称 (`task_name`) 和附加信息 (`addition_info`)
简言之，该脚本先后涉及：参数输入 -> 实验配置 -> 环境变量设置 -> 执行文件
具体而言
- 首先，脚本定义了数据集路径 `dataset_path`
并设置了一些默认参数——相当于实验配置，如调试模式 (`DEBUG`) 和是否保存检查点 (`save_ckpt`)# Examples: # bash scripts/deploy_policy.sh idp3 gr1_dex-3d 0913_example # bash scripts/deploy_policy.sh dp_224x224_r3m gr1_dex-image 0913_example # $1: alg_name - 算法名称，例如 "idp3" 或 "dp_224x224_r3m" # $2: task_name - 任务名称，例如 "gr1_dex-3d" 或 "gr1_dex-image" # $3: addition_info - 附加信息，用于标记实验，例如 "0913_example" dataset_path=/home/ze/projects/Improved-3D-Diffusion-Policy/training_data_exampleDEBUG=False save_ckpt=True - 接着，设定实验名称与目录
即它从命令行参数中获取算法名称、任务名称和附加信息，并使用这些信息生成实验名称 (`exp_name`) 和运行目录 (`run_dir`)
脚本还设置了 GPU 的 ID——相当于GPU设置，并打印出使用的 GPU IDalg_name=${1} task_name=${2} config_name=${alg_name} addition_info=${3} seed=0 exp_name=${task_name}-${alg_name}-${addition_info} run_dir="data/outputs/${exp_name}_seed${seed}"gpu_id=0 echo -e "\033[33mgpu id (to use): ${gpu_id}\033[0m" - 然后，它切换到 Improved-3D-Diffusion-Policy 目录
并设置一些环境变量，如 `HYDRA_FULL_ERROR` 和 `CUDA_VISIBLE_DEVICES`cd Improved-3D-Diffusion-Policyexport HYDRA_FULL_ERROR=1 export CUDA_VISIBLE_DEVICES=${gpu_id} - 最后，脚本运行 `deploy.py` 脚本——下节即将介绍，并传递多个参数，包括配置文件名称、任务名称、运行目录、调试模式、随机种子、设备、实验名称、日志模式、是否保存检查点以及数据集路径
这些参数用于配置和运行模型的部署过程# 使用指定的配置文件运行 deploy.py 脚本 python deploy.py --config-name=${config_name}.yaml \ task=${task_name} \ # 设置任务名称 hydra.run.dir=${run_dir} \ # 设置运行目录 training.debug=$DEBUG \ # 设置调试模式 training.seed=${seed} \ # 设置随机种子 training.device="cuda:0" \ # 设置训练设备为 GPU exp_name=${exp_name} \ # 设置实验名称 logging.mode=${wandb_mode} \ # 设置日志模式 checkpoint.save_ckpt=${save_ckpt} \ # 设置是否保存检查点 task.dataset.zarr_path=$dataset_path # 设置数据集路径
如姚博士所说，需要注意以下几点
- wandb_mode 未在脚本中定义：需要在执行脚本前定义 wandb_mode 变量，否则会报错
- 路径校验：确保 dataset_path 和 Improved-3D-Diffusion-Policy 目录存在
- GPU 可见性：确保系统中存在指定的 GPU 设备
3.2 Improved-3D-Diffusion-Policy/deploy.py：部署全流程
作为机器人系统控制和推理部署脚本，主要基于扩散策略模型（Diffusion Policy Model）进行推理和决策，包括以下4个核心功能：
- 机器人控制：通过 UpperBodyCommunication 和 HandCommunication 来控制机器人的上半身关节和机械手
- 环境感知：使用 RealSense 相机获取深度图像、RGB图像和点云数据，提供给深度学习模型作为输入
- 动作推理：加载预训练的策略模型，根据实时环境输入推理出下一步的控制动作
- 数据记录：在执行任务的同时记录传感器数据和机器人关节状态，以便后续分析或调试
3.2.1 各种库函数的应用：含通讯模块zenoh
首先，导入机器人控制、相机控制、图像处理和扩散策略操作所需的模块和库，其中
- 首先
import sys # 使用行缓冲模式设置 stdout 和 stderr sys.stdout = open(sys.stdout.fileno(), mode='w', buffering=1) sys.stderr = open(sys.stderr.fileno(), mode='w', buffering=1) - 其次
import hydra # 导入 hydra 库，用于配置管理 import time # 导入 time 库，用于时间相关操作 from omegaconf import OmegaConf # 导入 OmegaConf 库，用于配置解析 import pathlib # 导入 pathlib 库，用于路径操作 # 导入 BaseWorkspace 类 from diffusion_policy_3d.workspace.base_workspace import BaseWorkspace # 导入动作工具模块并重命名为 action_util import diffusion_policy_3d.common.gr1_action_util as action_util # 导入旋转工具模块并重命名为 rotation_util import diffusion_policy_3d.common.rotation_util as rotation_util import tqdm # 导入 tqdm 库，用于显示进度条 import torch # 导入 torch 库，用于深度学习 import os # 导入 os 库，用于操作系统相关功能 os.environ['WANDB_SILENT'] = "True" # 设置环境变量，静默 wandb 输出 # 允许在配置中执行任意 Python 代码 OmegaConf.register_new_resolver("eval", eval, replace=True) - 最后
其中，zenoh 是仿人机器人通讯（UpperBody 及 Hand ）相关模块（Humanoid-Teleoperation）# 导入 MultiRealSense 类 from diffusion_policy_3d.common.multi_realsense import MultiRealSense # 设置 zenoh 路径 zenoh_path="/home/gr1p24ap0049/projects/gr1-dex-real/teleop-zenoh" sys.path.append(zenoh_path) # 将 zenoh 路径添加到系统路径中 from communication import * # 导入 communication 模块中的所有内容 from retarget import ArmRetarget # 导入 ArmRetarget 类 import numpy as np # 导入 numpy 库并重命名为 np import torch # 再次导入 torch 库（可能是重复导入） from termcolor import cprint # 导入 cprint 函数，用于彩色打印
如合伙人姚博士所说，此模块是 iDP3 复现使用和外界通讯的关键脚本，如果想替换对应硬件，则需要更改此脚本
3.2.2 类 GR1DexEnvInference：用于机器人部署
这段代码定义了一个名为 GR1DexEnvInference 的类，用于在机器人本地计算机上运行的部署环境。该类主要负责初始化机器人环境、执行动作、获取观测数据、重置环境。其中：
- __init__ 方法初始化了机器人环境的各个组件
比如包括相机class GR1DexEnvInference: """ 部署在机器人的本地计算机上运行 """ def __init__(self, obs_horizon=2, action_horizon=8, device="gpu", use_point_cloud=True, use_image=True, img_size=224, num_points=4096, use_waist=False): # 观测/动作 self.use_point_cloud = use_point_cloud # 是否使用点云 self.use_image = use_image # 是否使用图像 self.use_waist = use_waist # 是否使用腰部
时间跨度# 相机 self.camera = MultiRealSense(use_front_cam=True, # 默认使用单个相机，但也支持多相机 front_num_points=num_points, img_size=img_size)
推理设备# 时间跨度 self.obs_horizon = obs_horizon # 观测时间跨度 self.action_horizon = action_horizon # 动作时间跨度
通信接口# 推理设备 if device == "gpu": self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") # 使用 GPU 或 CPU else: self.device = torch.device("cpu") # 使用 CPU# 机器人通信 self.upbody_comm = UpperBodyCommunication() # 上半身通信 self.hand_comm = HandCommunication() # 手部通信 self.arm_solver = ArmRetarget("AVP") # 手臂重定向器 - step 方法执行一系列动作，并收集相应的观测数据
简言之，它将动作发送到机器人，并从相机和传感器获取数据，最后返回一个包含观测数据的字典
具体而言
首先，方法遍历 action_list 中的每个动作，并将其转换为32个关节的格式
然后，将动作分为位置和手部位置两个部分。如果不使用腰部，则将位置的前六个值设为零def step(self, action_list): for action_id in range(self.action_horizon): act = action_list[action_id] # 获取当前动作 self.action_array.append(act) # 将动作添加到动作数组中 act = action_util.joint25_to_joint32(act) # 将25关节动作转换为32关节动作
接着，方法通过 upbody_comm 和 hand_comm 分别设置上半身和手部的位置filtered_act = act.copy() # 复制动作 filtered_pos = filtered_act[:-12] # 获取过滤后的位置 filtered_handpos = filtered_act[-12:] # 获取过滤后的手部位置 if not self.use_waist: filtered_pos[0:6] = 0. # 如果不使用腰部，将腰部位置设为0
在每个动作步骤中，方法还会从相机获取点云、颜色和深度数据，并将这些数据存储在相应的数组中self.upbody_comm.set_pos(filtered_pos) # 设置上半身位置 self.hand_comm.send_hand_cmd(filtered_handpos[6:], filtered_handpos[:6]) # 发送手部命令
同时，方法尝试获取手部的关节位置，如果失败，则使用默认值。然后，将上半身和手部的关节位置合并，并存储在环境关节位置数组中cam_dict = self.camera() # 获取相机数据 self.cloud_array.append(cam_dict['point_cloud']) # 添加点云数据到数组中 self.color_array.append(cam_dict['color']) # 添加颜色数据到数组中 self.depth_array.append(cam_dict['depth']) # 添加深度数据到数组中
最后，方法将最近的观测数据堆叠起来，形成一个包含代理位置、点云和图像的字典 obs_dict。这些数据被转换为 PyTorch 张量，并根据需要移动到指定的设备（如 GPU）try: hand_qpos = self.hand_comm.get_qpos() # 获取手部位置 except: # 获取手部位置失败，使用默认值 cprint("fail to fetch hand qpos. use default.", "red") hand_qpos = np.ones(12) # 默认手部位置为全1 # 获取环境位置 env_qpos = np.concatenate([self.upbody_comm.get_pos(), hand_qpos]) self.env_qpos_array.append(env_qpos) # 添加环境位置到数组中
最终，方法返回这个包含观测数据的字典agent_pos = np.stack(self.env_qpos_array[-self.obs_horizon:], axis=0) # 获取最近的观测位置 # 获取最近的点云数据 obs_cloud = np.stack(self.cloud_array[-self.obs_horizon:], axis=0) # 获取最近的图像数据 obs_img = np.stack(self.color_array[-self.obs_horizon:], axis=0) obs_dict = { # 将观测位置转换为张量 'agent_pos': torch.from_numpy(agent_pos).unsqueeze(0).to(self.device), } if self.use_point_cloud: # 如果用点云数据，则将点云数据转换为张量 obs_dict['point_cloud'] = torch.from_numpy(obs_cloud).unsqueeze(0).to(self.device) if self.use_image: # 如果用图像数据，则将图像数据转换为张量 obs_dict['image'] = torch.from_numpy(obs_img).permute(0, 3, 1, 2).unsqueeze(0)return obs_dict # 返回观测字典 - reset 方法 重置机器人和环境，包括相机和传感器，并返回初始观测数据
该方法接受一个布尔参数 first_init，用于指示是否是第一次初始化
首先，方法初始化了一些缓冲区，包括颜色数组、深度数组、点云数组、环境关节位置数组和动作数组def reset(self, first_init=True):
接着，定义了两个初始关节位置数组 qpos_init1 和 qpos_init2，以及一个手部初始位置数组 hand_init# 初始化缓冲区 self.color_array, self.depth_array, self.cloud_array = [], [], [] # 初始化颜色、深度和点云数组 self.env_qpos_array = [] # 初始化环境关节位置数组 self.action_array = [] # 初始化动作数组
如果 first_init 为真，方法会将 qpos_init2 连接起来，并通过 upbody_comm 和hand_comm 分别设置上半身和手部的位置# 位置初始化 qpos_init1 = np.array([-np.pi / 12, 0, 0, -1.6, 0, 0, 0, -np.pi / 12, 0, 0, -1.6, 0, 0, 0]) # 初始关节位置1 qpos_init2 = np.array([-np.pi / 12, 0, 1.5, -1.6, 0, 0, 0, -np.pi / 12, 0, -1.5, -1.6, 0, 0, 0]) # 初始关节位置2 hand_init = np.ones(12) # 初始手部位置为全1 # hand_init = np.ones(12) * 0 # 初始手部位置为全0（注释掉）
然后，将 qpos_init1 连接起来，并再次设置上半身的位置if first_init: # ======== 初始化 ========== upbody_initpos = np.concatenate([qpos_init2]) # 连接初始关节位置2 self.upbody_comm.init_set_pos(upbody_initpos) # 设置上半身初始位置 self.hand_comm.send_hand_cmd(hand_init[6:], hand_init[:6]) # 发送手部初始命令
接着，方法使用逆运动学（IK）求解器 arm_solver 对齐末端执行器（EEF）的位置，并设置上半身的位置upbody_initpos = np.concatenate([qpos_init1]) # 连接初始关节位置1 self.upbody_comm.init_set_pos(upbody_initpos) # 设置上半身初始位置 q_14d = upbody_initpos.copy() # 复制上半身初始位置 body_action = np.zeros(6) # 初始化身体动作为全0
在完成这些初始化步骤后，方法暂停两秒钟，并打印 "Robot ready!" 以指示机器人已准备好# 末端执行器位置对齐 arm_pos, arm_rot_quat = action_util.init_arm_pos, action_util.init_arm_quat # 获取手臂初始位置和旋转四元数 q_14d = self.arm_solver.ik(q_14d, arm_pos, arm_rot_quat) # 通过逆运动学计算手臂位置 self.upbody_comm.init_set_pos(q_14d) # 设置上半身位置
然后，方法从相机获取颜色、深度和点云数据，并将这些数据存储在相应的数组中time.sleep(2) # 等待2秒 print("Robot ready!") # 打印机器人准备好信息
接着，方法尝试获取手部的关节位置，如果失败，则使用默认值# ======== 初始化相机 ========== cam_dict = self.camera() # 获取相机数据 self.color_array.append(cam_dict['color']) # 添加颜色数据到数组中 self.depth_array.append(cam_dict['depth']) # 添加深度数据到数组中 self.cloud_array.append(cam_dict['point_cloud']) # 添加点云数据到数组中
然后，将上半身和手部的关节位置合并，并存储在环境关节位置数组中try: hand_qpos = self.hand_comm.get_qpos() # 获取手部位置 except: cprint("fail to fetch hand qpos. use default.", "red") # 获取手部位置失败，使用默认值 hand_qpos = np.ones(12) # 默认手部位置为全1
最后，方法将最近的观测数据堆叠起来，形成一个包含代理位置、点云和图像的字典 obs_dictenv_qpos = np.concatenate([self.upbody_comm.get_pos(), hand_qpos]) # 获取环境位置 self.env_qpos_array.append(env_qpos) # 添加环境位置到数组中 self.q_14d = q_14d # 设置上半身位置 self.body_action = body_action # 设置身体动作
这些数据被转换为 PyTorch 张量，并根据需要移动到指定的设备（如 GPU）
最终，方法返回这个包含观测数据的字典
通过以上这些方法，可以控制机器人执行任务并收集相关数据
3.2.3 主函数main
简言之，这是整个程序的核心入口，用于初始化配置、加载模型和环境，执行推理控制循环，并记录运行数据
- 初始化配置：使用 Hydra 动态加载配置文件和策略模型，解析任务相关的参数；设置随机种子，确保推理结果可重复
- 加载模型和环境：创建环境对象，设定观测和动作的时间步长；加载策略模型，用于从观测数据中生成动作
- 执行推理控制循环：根据不同的任务类型（grasp, pour, wipe）设定推理执行的步数，初始化环境 (GR1DexEnvInference) 并重置机器人状态，循环执行推理和动作控制，调用策略模型生成动作，执行动作并更新环境观察值，持续到设定的 roll_out_length 为止
- 记录运行数据（可选）：如果 record_data=True，将采集到的数据保存到 HDF5 文件中，并支持用户选择文件重命名
具体而言
- 首先，做Hydra 配置管理
Hydra 用于从配置文件中加载参数
config_path 指定了配置文件的路径，位于 diffusion_policy_3d/config 目录下——同样的，在此文《iDP3的Learning代码解析：逐步分解人形策略斯坦福iDP3的数据集、模型、动作预测策略代码(包含2D和3D两个版本)》的「1.2 diffusion_policy_3d/config」已介绍
函数的参数 cfg 是 OmegaConf 对象，包含了从配置文件加载的所有配置信息@hydra.main( config_path=str(pathlib.Path(__file__).parent.joinpath( 'diffusion_policy_3d','config')) # 配置文件路径 ) def main(cfg: OmegaConf): # 使用 Hydra 管理配置，加载指定路径下的配置文件 # config_path：配置文件所在目录，定义了程序的各类参数 # cfg：OmegaConf 对象，包含从配置文件加载的所有参数 - 设置随机种子和解析配置
torch.manual_seed(42)：设置 PyTorch 的随机种子，保证实验结果具有可重复性
OmegaConf.resolve(cfg)：解析配置文件中的动态变量（如 ${now:}、${eval:}），并替换为实际值torch.manual_seed(42) # 设置随机种子# 立即解析配置，以便所有 ${now:} 解析器使用相同的时间 OmegaConf.resolve(cfg) - 加载工作区对象
cfg._target_：配置文件中定义的类路径（字符串形式），通过 hydra.utils.get_class 动态加载相应的类
workspace：BaseWorkspace 类的实例，封装了与策略模型加载和环境交互相关的逻辑cls = hydra.utils.get_class(cfg._target_) # 获取目标类workspace: BaseWorkspace = cls(cfg) # 实例化工作空间 - 判断输入类型
根据 workspace 的类名判断当前任务类型：如果是 DPWorkspace，则任务需要图像输入，不使用点云；如果是其他类型，则任务需要点云输入，不使用图像if workspace.__class__.__name__ == 'DPWorkspace': use_image = True # 使用图像 use_point_cloud = False # 不使用点云 else: use_image = False # 不使用图像 use_point_cloud = True # 使用点云 - 加载策略模型和推理参数
其中# 获取策略模型 policy = workspace.get_model() action_horizon = policy.horizon - policy.n_obs_steps + 1 # 动作时间跨度
policy：从 workspace 加载策略模型，用于根据观察值生成动作
action_horizon：实际执行的动作时间步长
policy.horizon：策略模型的预测时间范围
policy.n_obs_steps：策略模型需要的观察步数 - 设置任务参数
任务类型包括 pour（倒液体）、grasp（抓取）、wipe（擦拭）
根据任务类型设置 roll_out_length，即推理和控制的总步数# 任务配置 roll_out_length_dict = { "pour": 300, "grasp": 1000, "wipe": 300, } # task = "wipe" task = "grasp" # 任务类型——grasp # task = "pour" roll_out_length = roll_out_length_dict[task] # 任务执行长度 img_size = 224 # 图像大小 num_points = 4096 # 点云数量 use_waist = True # 是否使用腰部 first_init = True # 是否第一次初始化 record_data = True # 是否记录数据 - 初始化环境
其中env = GR1DexEnvInference(obs_horizon=2, action_horizon=action_horizon, device="cpu", use_point_cloud=use_point_cloud, use_image=use_image, img_size=img_size, num_points=num_points, use_waist=use_waist) # 初始化环境 obs_dict = env.reset(first_init=first_init) # 重置环境
obs_horizon=2：设置观察的时间步长
action_horizon=action_horizon：设置动作的时间步长
device="cpu"：使用 CPU 进行推理
use_point_cloud 和 use_image：控制是否使用点云或图像输入
最后，调用 reset 方法初始化机器人和传感器数据，返回初始观测字典 obs_dict - 推理和控制循环（主循环）
相当于step_count = 0 # 步数计数器 while step_count < roll_out_length: with torch.no_grad(): # 禁用梯度计算 action = policy(obs_dict)[0] # 获取动作 # 将动作转换为 numpy 数组 action_list = [act.numpy() for act in action] obs_dict = env.step(action_list) # 执行动作 step_count += action_horizon # 更新步数计数器 print(f"step: {step_count}") # 打印当前步数
每次迭代中，调用策略模型生成动作
使用 env.step 执行动作，并采集新的观测数据
将步数增加 action_horizon，直到达到 roll_out_length
禁用梯度计算：torch.no_grad() 减少内存消耗，提高推理速度 - 数据记录
以上相当于if record_data: # 导入 h5py 库 import h5py root_dir = "/home/gr1p24ap0049/projects/gr1-learning-real/" # 根目录 save_dir = root_dir + "deploy_dir" # 保存目录 os.makedirs(save_dir, exist_ok=True) # 创建保存目录 record_file_name = f"{save_dir}/demo.h5" # 记录文件名 color_array = np.array(env.color_array) # 颜色数组 depth_array = np.array(env.depth_array) # 深度数组 cloud_array = np.array(env.cloud_array) # 点云数组 qpos_array = np.array(env.qpos_array) # 关节位置数组 with h5py.File(record_file_name, "w") as f: # 创建 h5 文件 f.create_dataset("color", data=np.array(color_array)) # 创建颜色数据集 f.create_dataset("depth", data=np.array(depth_array)) # 创建深度数据集 f.create_dataset("cloud", data=np.array(cloud_array)) # 创建点云数据集 f.create_dataset("qpos", data=np.array(qpos_array)) # 创建关节位置数据集
使用 h5py 将采集的数据保存到 HDF5 文件中
数据包括 RGB 图像、深度图、点云和机器人关节状态
默认保存到 demo.h5
如果用户选择重命名，会更改文件名 - 文件重命名
以上相当于choice = input("whether to rename: y/n") # 是否重命名文件 if choice == "y": renamed = input("file rename:") # 输入新文件名 # 重命名文件 os.rename(src=record_file_name, dst=record_file_name.replace("demo.h5", renamed+'.h5')) new_name = record_file_name.replace("demo.h5", renamed+'.h5') # 新文件名 cprint(f"save data at step: {roll_out_length} in {new_name}", "yellow") # else: cprint(f"save data at step: {roll_out_length} in {record_file_name}", "yellow")
提示用户是否需要重命名文件
如果选择 y，则输入新的文件名，完成重命名
输出保存文件的最终路径
3.3 Realsense L515 测试脚本
iDP3 采用了 realsense L515 激光雷达相机，但是并没有相关测试脚本，这一点 UMI 做的相对舒服一些，方便问题排查，因此姚博士在复现过程中写了一些测试脚本，开源方便大家复现少点麻烦
首先安装 python api，此处注意务必采用推荐的 pyrealsense2==2.54.2.5684 版本，因为 L515 作为一款老产品新版本未经过适配测试
pip install pyrealsense2==2.54.2.5684
pip install opencv-python然后，在 idp3 环境中运行以下脚本，此脚本根据 realsense 官方文档进行了修改，设置了三个窗口显示：
- RGB图像
- 伪彩色深度图像
- 原始灰度深度图像
以下是部分代码「完整代码将放在七月官网首页的具身智能复现实战训练营 第二期中」
- 创建三个窗口
其中cv2.namedWindow("RealSense Color", cv2.WINDOW_AUTOSIZE) cv2.namedWindow("RealSense Depth (Color Map)", cv2.WINDOW_AUTOSIZE) cv2.namedWindow("RealSense Depth (Raw)", cv2.WINDOW_AUTOSIZE)
RealSense Color: 显示RGB图像
RealSense Depth (Color Map): 显示伪彩色处理后的深度图像
RealSense Depth (Raw): 显示原始深度图像 - 其次
try: while True: # 等待获取新的一帧 frames = pipeline.wait_for_frames() # 获取深度和颜色图像帧 depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue # 转换图像为numpy数组 depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data()) - 伪彩色深度图像
# 对深度图像进行伪彩色处理，使其更容易查看 depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET) - 显示RGB图像、伪彩色深度图像和原始深度图像
# 使用cv2.imshow()来显示RGB图像、伪彩色深度图像和原始深度图像 cv2.imshow("RealSense Color", color_image) cv2.imshow("RealSense Depth (Color Map)", depth_colormap) cv2.imshow("RealSense Depth (Raw)", depth_image) - 最后
# 等待按键，按“q”退出 key = cv2.waitKey(1) if key == ord('q'): break finally: # 停止pipeline pipeline.stop() # 关闭所有OpenCV窗口 cv2.destroyAllWindows()
运行该程序后弹出三个窗口，以通过按 “q” 键退出程序(下图来自合伙人姚博士)
至于更多我司对iDP3通用化的改写，后续发布或见七月官网首页的具身智能复现实战训练营
扫一扫
808
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
