ASAP适配宇树H1机器人
🚀 ASAP适配宇树H1机器人
📋 简介
本文介绍如何使用ASAP技术对宇树H1机器人进行动作捕捉和数据转换。我们将重点关注从视频输入到SMPL格式数据输出的整个流程，特别是如何将人体姿态估计结果转换为可用于机器人控制的NPZ格式文件。
🔄 整体流程
整体流程分为以下几个步骤：
- 视频转SMPL数据 - 获取PBHC开源项目代码以及GVHMR开源项目代码，借用PBHC其中motion_source目录下的demo.py代码将其拷贝到GVHMR项目目录下的tools/demo目录下
- 机器人配置 - 编写对应的H1机器人的仿真配置yaml文件
- 形状参数拟合 - 使用ASAP项目对smpl动作文件进行形状参数拟合
- 运动数据重定向 - 将smpl运动数据重定向到人形机器人
- 动作效果预览 - 查看重定向后的机器人动作效果
- 强化学习训练 - 编写强化学习奖励函数使其学会在平面地形下的稳定行走能力
- 模型评估与转换 - 预览训练出来的模型效果并转化为onnx模型
- 复杂动作训练 - 训练自定义的复杂动作控制策略模型
- 仿真测试 - 在仿真中运行训练好的动作模型控制策略
- 真机部署 - 将模型部署到真实机器人上
🎬 步骤1: 视频转SMPL数据
此代码实现将视频转化为标准的smpl格式的npz动作文件，同时也会生成可视化的视频文件。
📄 demo_to_npz.py
import cv2
import torch
import pytorch_lightning as pl
import numpy as np
import argparse
from hmr4d.utils.pylogger import Log
import hydra
from hydra import initialize_config_module, compose
from pathlib import Path
from pytorch3d.transforms import quaternion_to_matrix
from hmr4d.configs import register_store_gvhmr
from hmr4d.utils.video_io_utils import (
get_video_lwh,
read_video_np,
save_video,
merge_videos_horizontal,
get_writer,
get_video_reader,
)
from hmr4d.utils.vis.cv2_utils import draw_bbx_xyxy_on_image_batch, draw_coco17_skeleton_batch
from hmr4d.utils.preproc import Tracker, Extractor, VitPoseExtractor, SimpleVO
from hmr4d.utils.geo.hmr_cam import get_bbx_xys_from_xyxy, estimate_K, convert_K_to_K4, create_camera_sensor
from hmr4d.utils.geo_transform import compute_cam_angvel
from hmr4d.model.gvhmr.gvhmr_pl_demo import DemoPL
from hmr4d.utils.net_utils import detach_to_cpu, to_cuda
from hmr4d.utils.smplx_utils import make_smplx
from hmr4d.utils.vis.renderer import Renderer, get_global_cameras_static, get_ground_params_from_points
from tqdm import tqdm
from hmr4d.utils.geo_transform import apply_T_on_points, compute_T_ayfz2ay
from einops import einsum, rearrange
from scipy.spatial.transform import Rotation as sRot
CRF = 23 # 17 is lossless, every +6 halves the mp4 size
def parse_args_to_cfg():
# Put all args to cfg
parser = argparse.ArgumentParser()
parser.add_argument("--video", type=str, default="inputs/demo/dance_3.mp4")
parser.add_argument("--output_root", type=str, default=None, help="by default to outputs/demo")
parser.add_argument("-s", "--static_cam", action="store_true", help="If true, skip DPVO")
parser.add_argument("--use_dpvo", action="store_true", help="If true, use DPVO. By default not using DPVO.")
parser.add_argument(
"--f_mm",
type=int,
default=None,
help="Focal length of fullframe camera in mm. Leave it as None to use default values."
"For iPhone 15p, the [0.5x, 1x, 2x, 3x] lens have typical values [13, 24, 48, 77]."
"If the camera zoom in a lot, you can try 135, 200 or even larger values.",
)
parser.add_argument("--verbose", action="store_true", help="If true, draw intermediate results")
args = parser.parse_args()
# Input
video_path = Path(args.video)
assert video_path.exists(), f"Video not found at {video_path}"
length, width, height = get_video_lwh(video_path)
Log.info(f"[Input]: {video_path}")
Log.info(f"(L, W, H) = ({length}, {width}, {height})")
# Cfg
with initialize_config_module(version_base="1.3", config_module=f"hmr4d.configs"):
overrides = [
f"video_name={video_path.stem}",
f"static_cam={args.static_cam}",
f"verbose={args.verbose}",
f"use_dpvo={args.use_dpvo}",
]
if args.f_mm is not None:
overrides.append(f"f_mm={args.f_mm}")
# Allow to change output root
if args.output_root is not None:
overrides.append(f"output_root={args.output_root}")
register_store_gvhmr()
cfg = compose(config_name="demo", overrides=overrides)
# Output
Log.info(f"[Output Dir]: {cfg.output_dir}")
Path(cfg.output_dir).mkdir(parents=True, exist_ok=True)
Path(cfg.preprocess_dir).mkdir(parents=True, exist_ok=True)
# Copy raw-input-video to video_path
Log.info(f"[Copy Video] {video_path} -> {cfg.video_path}")
if not Path(cfg.video_path).exists() or get_video_lwh(video_path)[0] != get_video_lwh(cfg.video_path)[0]:
reader = get_video_reader(video_path)
writer = get_writer(cfg.video_path, fps=30, crf=CRF)
for img in tqdm(reader, total=get_video_lwh(video_path)[0], desc=f"Copy"):
writer.write_frame(img)
writer.close()
reader.close()
return cfg
🤖 步骤2: H1机器人配置
拉取ASAP项目源代码，将上一个生成的NPZ文件拷贝到humanoidverse/data/motion/raw_tairantestbed_smpl（也可以自己在motion目录下新建一个目录放进去），然后我们就需要编写对应的H1机器人的仿真配置yaml。
📄 humanoidverse/config/robot/h1/h1.yaml
# @package _global_
defaults:
- robot_base
robot:
# Observation parameters
num_bodies: 20 # 根据H1的实际身体数量调整
dof_obs_size: 19 # 根据H1的实际DOF数量调整
actions_dim: 19 # 根据H1的实际DOF数量调整
lower_body_actions_dim: 11 # 腿部动作维度
upper_body_actions_dim: 8 # 上身动作维度
policy_obs_dim: -1
critic_obs_dim: -1
key_bodies: ["left_ankle_link", "right_ankle_link"]
contact_bodies: ["left_ankle_link", "right_ankle_link"]
num_feet: 2
right_foot_name: "right_ankle_link"
left_foot_name: "left_ankle_link"
foot_name: "ankle_link"
knee_name: "knee_link"
has_torso: True
torso_name: "torso_link"
has_upper_body_dof: True
# 关节名称配置
dof_names: [
'left_hip_yaw_joint', 'left_hip_roll_joint', 'left_hip_pitch_joint', 'left_knee_joint', 'left_ankle_joint',
'right_hip_yaw_joint', 'right_hip_roll_joint', 'right_hip_pitch_joint', 'right_knee_joint', 'right_ankle_joint',
'torso_joint',
'left_shoulder_pitch_joint', 'left_shoulder_roll_joint', 'left_shoulder_yaw_joint', 'left_elbow_joint',
'right_shoulder_pitch_joint', 'right_shoulder_roll_joint', 'right_shoulder_yaw_joint', 'right_elbow_joint'
]
# 关节限制配置
dof_pos_lower_limit_list: [
-0.43, -0.43, -1.57, -0.26, -0.87, # 左腿
-0.43, -0.43, -1.57, -0.26, -0.87, # 右腿
-2.35, # 躯干
-2.87, -0.34, -1.3, -1.25, # 左臂
-2.87, -3.11, -4.45, -1.25 # 右臂
]
dof_pos_upper_limit_list: [
0.43, 0.43, 1.57, 2.05, 0.52, # 左腿
0.43, 0.43, 1.57, 2.05, 0.52, # 右腿
2.35, # 躯干
2.87, 3.11, 4.45, 2.61, # 左臂
2.87, 0.34, 1.3, 2.61 # 右臂
]
# 控制参数
control:
control_type: P
stiffness: # [N*m/rad]
hip_yaw: 100
hip_roll: 100
hip_pitch: 100
knee: 200
ankle: 20
torso: 400
shoulder_pitch: 90
shoulder_roll: 60
shoulder_yaw: 20
elbow: 60
damping: # [N*m*s/rad]
hip_yaw: 2.5
hip_roll: 2.5
hip_pitch: 2.5
knee: 5.0
ankle: 0.2
torso: 5.0
shoulder_pitch: 2.0
shoulder_roll: 1.0
shoulder_yaw: 0.4
elbow: 1.0
action_scale: 0.25
action_clip_value: 100.0
clip_torques: True
💡 注意: 此yaml中有用到h1机器人对应的urdf、usd、xml等文件可以从官方开源项目中获取，我本人是借鉴PHC开源项目中的phc/data/robot/unitree_h1的文件内容。
📊 步骤3: 形状参数拟合
使用ASAP项目目录下的scripts/data_process/fit_smpl_shape.py代码对smpl动作文件进行形状参数拟合：
python scripts/data_process/fit_smpl_shape.py +robot=h1/h1
🎯 步骤4: 运动数据重定向
使用ASAP项目目录下的scripts/data_process/fit_smpl_motion.py代码对将smpl运动数据重定向到人形机器人：
python scripts/data_process/fit_smpl_motion.py +robot=h1/h1
👁️ 步骤5: 动作效果预览
使用ASAP项目目录下的scripts/vis/vis_q_mj.py代码查看重定向后的机器人动作效果：
python scripts/vis/vis_q_mj.py +robot=h1/h1 +visualize_motion_file="humanoidverse/data/motions/h1/TairanTestbed/singles/0-motions_raw_tairantestbed_smpl_video_side_jump_level4_filter_amass.pkl"
🧠 步骤6: 强化学习训练
编写强化学习奖励函数使其学会在平面地形下的稳定行走能力，训练一个优化后的控制策略模型，此模型即为后面会使用到的loco_model。
📄 奖励函数配置
# @package _global_
rewards:
set_reward: Unitree_RL GYM EXACT REWARD
set_reward_date: 20241008
only_positive_rewards: False
reward_scales:
tracking_lin_vel: 1.0
tracking_ang_vel: 0.5
penalty_lin_vel_z: -2.0
penalty_ang_vel_xy: -1.0
penalty_orientation: -1.0
penalty_torques: -0.00001
penalty_dof_acc: -3.5e-8
penalty_action_rate: -0.01
penalty_feet_ori: -2.0
limits_dof_pos: -10.0
feet_air_time: 1.0
base_height: -100.0
penalty_feet_height: -5.0
reward_tracking_sigma:
lin_vel: 0.25
ang_vel: 0.25
# 其他配置...
feet_height_target: 0.2
locomotion_max_contact_force: 200.0
desired_feet_max_height_for_this_air: 0.2
desired_base_height: 0.98
🔍 步骤7: 模型评估与转换
使用humanoidverse/eval_agent.py预览我们训练出来的模型效果，同时将这个模型输出转化为onnx模型供后面使用：
python humanoidverse/eval_agent.py +checkpoint=logs/TestIsaacGymInstallation/20260120_172220-h1_loco-locomotion-h1/model_800.pt
🎭 步骤8: 复杂动作训练
使用第4步代码生成的pkl动作文件，训练我们自定义的复杂动作控制策略模型：
python humanoidverse/train_agent.py \
+simulator=isaacgym \
+exp=motion_tracking \
+domain_rand=NO_domain_rand \
+rewards=motion_tracking/reward_motion_tracking_dm_2real \
+robot=h1/h1 \
+terrain=terrain_locomotion_plane \
+obs=motion_tracking/deepmimic_a2c_nolinvel_LARGEnoise_history \
num_envs=4096 \
project_name=MotionTracking \
experiment_name=MotionTracking_CR7 \
robot.motion.motion_file="humanoidverse/data/motions/h1/TairanTestbed/singles/0-TairanTestbed_TairanTestbed_CR7_video_CR7_level1_filter_amass.pkl" \
rewards.reward_penalty_curriculum=True \
rewards.reward_penalty_degree=0.00001 \
env.config.resample_motion_when_training=False \
env.config.termination.terminate_when_motion_far=True \
env.config.termination_curriculum.terminate_when_motion_far_curriculum=True \
env.config.termination_curriculum.terminate_when_motion_far_threshold_min=0.3 \
env.config.termination_curriculum.terminate_when_motion_far_curriculum_degree=0.000025 \
robot.asset.self_collisions=0
🎮 步骤9: 模型评估
训练完成后，我们可以使用humanoidverse/eval_agent.py预览我们训练出来的模型效果，同时将这个模型输出转化为onnx模型供后面使用：
python humanoidverse/eval_agent.py \
+checkpoint=logs/MotionTracking/xxxxxxxx_xxxxxxx-MotionTracking_CR7-motion_tracking-h1/model_5800.pt
🔬 步骤10: Sim2Sim仿真测试
sim2sim在仿真中运行训练好的动作模型控制策略，进入sim2real目录：
python sim_env/base_sim.py --config=config/h1_hist.yaml
📄 h1_hist.yaml配置
# 机器人配置
ROBOT_TYPE: 'h1' # Robot name
ROBOT_SCENE: "../humanoidverse/data/robots/h1/scene_h1.xml" # Robot scene
ROBOT: "../humanoidverse/data/robots/h1/h1.urdf" # Robot URDF file
ASSET_ROOT: "../humanoidverse/data/robots/h1" # Robot Asset Root
ASSET_FILE: "h1.urdf" # Robot Asset File
# 网络配置
DOMAIN_ID: 0 # Domain id
INTERFACE: "lo" # Yuanhang: in simulation, lo0 for mac, lo for linux
# 控制参数
USE_JOYSTICK: 0 # Simulate Unitree WirelessController
FREE_BASE: False
PRINT_SCENE_INFORMATION: True # Print link, joint and sensors information
ENABLE_ELASTIC_BAND: True # Virtual spring band, used for lifting h1
# 时间参数
SIMULATE_DT: 0.005 # Need to be larger than the runtime of viewer.sync()
VIEWER_DT: 0.02 # Viewer update time
# 历史配置
USE_SENSOR: False
USE_HISTORY: True
USE_HISTORY_LOCO: True
USE_HISTORY_MIMIC: True
# 控制增益
JOINT_KP: [
100, 100, 100, 200, 20, # Left leg
100, 100, 100, 200, 20, # Right leg
400, # Torso
90, 60, 20, 60, # Left arm
90, 60, 20, 60 # Right arm
]
JOINT_KD: [
2.5, 2.5, 2.5, 5, 0.2, # Left leg
2.5, 2.5, 2.5, 5, 0.2, # Right leg
5.0, # Torso
2.0, 1.0, 0.4, 1.0, # Left arm
2.0, 1.0, 0.4, 1.0 # Right arm
]
🎮 控制说明
然后在另一个终端中启动控制策略：
python rl_policy/deepmimic_dec_loco_height.py --config=config/h1_hist.yaml --loco_model_path=./models/dec_loco/20250109_231507-noDR_rand_history_loco_stand_height_noise-decoupled_locomotion-g1_29dof/model_6600.onnx --mimic_model_paths=./models/mimic
键盘控制
| 按键 | 功能 |
|---|---|
] |
激活locomotion策略 |
[ |
激活asap策略（基于相位的动作跟踪策略） |
; |
切换到asap策略 |
i |
使机器人回到初始位置 |
o |
紧急停止机器人 |
9 |
在mujoco查看器中释放机器人栈 |
= |
在locomotion策略中切换轻敲和行走 |
w/a/s/d |
控制线速度 |
q/e |
控制角速度 |
z |
将所有命令设置为零 |
🔧 步骤11-12: Sim2Real真机部署
🚀 进入底层控制模式
- 开机启动人形机器人，等待头部蓝灯常亮
- 按下
L2+R2组合键 - 按下
L2+A组合键 - 按下
L2+B组合键 - 通过网线将电脑与G1机器人连接，并按照这份文档配置网络
⚙️ 启动前准备
修改配置文件 config/h1_hist.yaml，将 INTERFACE 参数设置为 eth0（若使用Linux系统）—— 本质上就是你电脑中用于连接机器人、且在ifconfig中显示IP为192.168.123.xxx格式的网络接口名称。
🚀 启动策略程序
python rl_policy/deepmimic_dec_loco_height.py --config=config/h1_hist.yaml --loco_model_path=./models/dec_loco/20250109_231507-noDR_rand_history_loco_stand_height_noise-decoupled_locomotion-h1/model_6600.onnx --mimic_model_paths=./models/mimic
📈 步骤13-15: 模型微调与优化
📊 数据收集
如果需要微调动作策略使其动作更好、更稳定，就需要收集真机数据然后训练delta增量动作模型：
python rl_policy/listener_deltaa.py --config config/h1.yaml --exp_name delta_a_real_data
数据将保存在：./../humanoidverse/logs/delta_a_realdata/{exp_name}/{时间戳}/motion_{episode编号}.npz
🎯 Delta模型训练
使用收集的数据来训练delta增量动作模型：
python humanoidverse/train_agent.py \
+simulator=isaacgym \
+exp=train_delta_a_open_loop \
+domain_rand=NO_domain_rand \
+rewards=motion_tracking/delta_a/reward_delta_a_openloop \
+robot=h1/h1 \
+terrain=terrain_locomotion_plane \
+obs=delta_a/open_loop \
num_envs=5000 \
project_name=DeltaA_Training \
experiment_name=openloopDeltaA_training \
robot.motion.motion_file="<PATH_TO_YOUR_MOTION_FILE_WITH_ACTION_KEYNAME>" \
env.config.max_episode_length_s=1.0 \
rewards.reward_scales.penalty_minimal_action_norm=-0.1 \
+device=cuda:0 \
env.config.resample_motion_when_training=True \
env.config.resample_time_interval_s=10000
🔧 策略微调
使用delta增量动作模型来对我们的动作控制策略进行微调：
HYDRA_FULL_ERROR=1 \
python humanoidverse/train_agent.py \
+simulator=isaacgym \
+exp=train_delta_a_closed_loop \
algo.config.policy_checkpoint='<PATH_TO_YOUR_DELTA_A_MODEL>' \
+domain_rand=NO_domain_rand_finetune_with_deltaA \
+rewards=motion_tracking/reward_motion_tracking_dm_simfinetuning \
+robot=g1/g1_29dof_anneal_23dof \
+terrain=terrain_locomotion_plane \
+obs=delta_a/train_policy_with_delta_a \
num_envs=4096 \
project_name=DeltaA_Finetune \
experiment_name=finetune_with_deltaA \
robot.motion.motion_file="<PATH_TO_YOUR_MOTION_FILE>" \
+opt=wandb \
env.config.add_extra_action=True \
+checkpoint="<PATH_TO_YOUR_POLICY_TO_BE_FINETUNED>" \
domain_rand.push_robots=False \
env.config.noise_to_initial_level=1 \
rewards.reward_penalty_curriculum=True \
+device=cuda:0 \
algo.config.save_interval=5 \
algo.config.num_learning_iterations=1000
✅ 总结
最后可以使用第10-12步骤进行模型的真机部署或者仿真环境下的测试。
通过这个完整的流程，你可以：
- 🎥 将视频转换为SMPL格式的动作数据
- 🤖 配置宇树H1机器人的仿真环境
- 🎯 训练稳定的行走控制策略
- 🎭 实现复杂的动作跟踪
- 🔧 完成Sim2Real的部署
- 📈 通过数据收集和微调优化模型性能
希望这个指南对你有所帮助！🚀
浙公网安备 33010602011771号