RGB to 3D Action Tracking: Hyrbik+mjlab(2)
·
接着上一篇博客的工作https://blog.csdn.net/2302_77159195/article/details/161718260?spm=1001.2014.3001.5502
1.使用mjlab里面的debugline，画一个棍棒，查看在mjlab的空间下，我做的动作被看成了什么东西
要进行的工作：
（1）提取相对坐标：使用pred_xyz_24_struct，将每一帧的24个点都减去第0个点骨盆的坐标
（2）修正轴向差异 ：Hybrik输出的坐标是相机坐标系下的，而mjlab/mujocjo里面是世界坐标系
（3）定义关节点的连接关系
verify_upper_body.py:
问题：线段连接不正确
查看SMPL官方关节点定义
15 Head
|
12 Neck
|
14 L_Collar — 9 Spine3 — 13 R_Collar
/ | \
17 L_Shoulder | 16 R_Shoulder
| | |
19 L_Elbow 6 Spine2 18 R_Elbow
| | |
21 L_Wrist 3 Spine1 20 R_Wrist
| | |
23 L_Hand | 22 R_Hand
0 Pelvis
/ \
1 L_Hip 2 R_Hip
| |
4 L_Knee 5 R_Knee
| |
7 L_Ankle 8 R_Ankle
| |
10 L_Foot 11 R_Foot
代码里面连接逻辑没问题，是之前处理数据的时候，因为我们目的是上半身动作模仿，所以保留了上半身的关节，这样索引就更改了
更改之后的verify_upper_body.py
import os
import pickle
import numpy as np
# ==========================================
# 配置路径 (自动获取当前脚本所在目录，防止找不到文件)
# ==========================================
CURRENT_DIR = os.path.dirname(os.path.abspath(__file__))
PKL_PATH = os.path.join(CURRENT_DIR, "realsense_results", "res_realsense.pk")
# SMPL 24 关节点定义 (用于查找索引)
SMPL_JOINT_NAMES = [
"pelvis", "left_hip", "right_hip", "spine1", "left_knee", "right_knee",
"spine2", "left_ankle", "right_ankle", "spine3", "left_foot", "right_foot",
"neck", "left_collar", "right_collar", "head", "left_shoulder", "right_shoulder",
"left_elbow", "right_elbow", "left_wrist", "right_wrist", "left_hand", "right_hand"
]
def process_upper_body():
# 1. 加载数据
if not os.path.exists(PKL_PATH):
print(f"❌ 找不到数据文件: {PKL_PATH}")
return
with open(PKL_PATH, 'rb') as f:
data = pickle.load(f)
# 获取原始数据 (N, 24, 3)
raw_joints = data['pred_xyz_24_struct']
print(f"✅ 数据加载成功: {raw_joints.shape[0]} 帧")
# 2. 坐标系转换 (修正倒立 + 适配 MuJoCo)
mj_joints = np.zeros_like(raw_joints)
mj_joints[:, :, 0] = raw_joints[:, :, 2] # New X = Old Z
mj_joints[:, :, 1] = raw_joints[:, :, 0] # New Y = Old X
mj_joints[:, :, 2] = -raw_joints[:, :, 1] # New Z = -Old Y
# 3. 归一化位置 (让骨盆回到原点)
pelvis_pos = mj_joints[:, 0:1, :]
mj_joints_normalized = mj_joints - pelvis_pos
# 4. 验证结果
print("-" * 40)
print("🔍 [最终检查 - 上半身模式]")
frame_idx = 0
pelvis_z = mj_joints_normalized[frame_idx, 0, 2]
head_z = mj_joints_normalized[frame_idx, 15, 2] # Index 15 is Head
neck_z = mj_joints_normalized[frame_idx, 12, 2] # Index 12 is Neck
print(f" Pelvis Z (应为0): {pelvis_z:.4f}")
print(f" Neck Z (应为正): {neck_z:.4f}")
print(f" Head Z (应为正且最高): {head_z:.4f}")
if head_z > 0 and head_z > neck_z:
print(" ✅ 姿态正确：人是正立的，且头比脖子高。")
else:
print(" ❌ 姿态依然异常，请检查数据源。")
# 5. 【核心】按照指定的连线拓扑重新提取关键点 (共16个点)
upper_body_indices = [
# --- 中轴线 (0-3-6-9-12-15) ---
0, # 0: pelvis
3, # 1: spine1
6, # 2: spine2
9, # 3: spine3 (分支根节点)
12, # 4: neck
15, # 5: head
# --- 左臂链 (9-14-17-19-21-23) ---
14, # 6: left_collar
16, # 7: left_shoulder
18, # 8: left_elbow
20, # 9: left_wrist
22, # 10: left_hand
# --- 右臂链 (9-13-16-18-20-22) ---
13, # 11: right_collar
17, # 12: right_shoulder
19, # 13: right_elbow
21, # 14: right_wrist
23, # 15: right_hand
]
upper_body_data = mj_joints_normalized[:, upper_body_indices, :]
print(f"\n🚀 准备就绪: 已提取上半身数据，Shape: {upper_body_data.shape}")
# 6. 【新增】将处理好的数据保存到 npz 文件中
OUTPUT_PATH = os.path.join(CURRENT_DIR, "realsense_results", "upper_body_data.npz")
np.savez(OUTPUT_PATH, joints=upper_body_data)
print(f"💾 数据已成功保存至: {OUTPUT_PATH}")
if __name__ == "__main__":
process_upper_body()
debug_visualize3.py(建立在mjlab的项目下):
import os
import numpy as np
import mujoco
import mujoco.viewer
import time
# ==========================================
# 1. 配置与路径
# ==========================================
DATA_PATH = "/home/labuser/unitree_ws/src/HybrIK/realsense_results/upper_body_data.npz"
MODEL_PATH = "/home/labuser/unitree_ws/src/unitree_rl_mjlab/mjlab/asset_zoo/robots/unitree_g1/xmls/g1_invisible.xml"
HEIGHT_OFFSET = 0.8 # 抬升高度，防止陷进地里
# ==========================================
# 2. 【核心修改】严格按照 16 点数据的连接顺序
# ==========================================
SKELETON_EDGES_CUSTOM = [
# --- 躯干与头部链 (0-1-2-3-4-5) ---
(0, 1), (1, 2), (2, 3), (3, 4), (4, 5),
# --- 左侧分支 (3-6-7-8-9-10) ---
(3, 6), (6, 7), (7, 8), (8, 9), (9, 10),
# --- 右侧分支 (3-11-12-13-14-15) ---
(3, 11), (11, 12), (12, 13), (13, 14), (14, 15),
]
def main():
if not os.path.exists(DATA_PATH):
print(f"❌ 找不到数据文件: {DATA_PATH}")
return
data_dict = np.load(DATA_PATH)
joints = data_dict['joints']
num_frames, num_points, _ = joints.shape
print(f"✅ 数据加载成功: {num_frames} 帧, {num_points} 个点")
if not os.path.exists(MODEL_PATH):
print(f"❌ 找不到模型文件: {MODEL_PATH}")
return
model = mujoco.MjModel.from_xml_path(MODEL_PATH)
sim_data = mujoco.MjData(model)
print("✅ G1 隐形模型加载成功！")
with mujoco.viewer.launch_passive(model, sim_data) as viewer:
frame_idx = 0
while viewer.is_running() and frame_idx < num_frames:
step_start = time.time()
current_skeleton = joints[frame_idx].copy()
# === 坐标抬升 ===
current_skeleton[:, 2] += HEIGHT_OFFSET
# === 强制相机看向骨架中心 (解决看不见的问题) ===
center = np.mean(current_skeleton, axis=0)
with viewer.lock():
viewer.cam.lookat[:] = center
# 物理步进
mujoco.mj_step(model, sim_data)
# === 绘制 Debug 几何体 ===
viewer.user_scn.ngeom = 0
# A. 绘制所有关节点 (红球)
for i in range(num_points):
add_sphere(viewer, current_skeleton[i], radius=0.02, rgba=[1, 0, 0, 1])
# B. 绘制自定义连线 (蓝线)
for start_idx, end_idx in SKELETON_EDGES_CUSTOM:
# 安全检查：确保索引不越界
if start_idx < num_points and end_idx < num_points:
add_line(viewer, current_skeleton[start_idx], current_skeleton[end_idx], width=0.015, rgba=[0, 0, 1, 1])
viewer.sync()
elapsed = time.time() - step_start
sleep_time = max(0, 1.0 / 30.0 - elapsed)
time.sleep(sleep_time)
frame_idx += 1
if frame_idx % 10 == 0:
print(f"Playing frame: {frame_idx}/{num_frames} | Center: {center}")
# ==========================================
# 辅助绘图函数
# ==========================================
def add_sphere(viewer, pos, radius=0.05, rgba=[1, 0, 0, 1]):
MAX_GEOM_LIMIT = 10000
if viewer.user_scn.ngeom >= MAX_GEOM_LIMIT: return
geom = viewer.user_scn.geoms[viewer.user_scn.ngeom]
mujoco.mjv_initGeom(geom, type=mujoco.mjtGeom.mjGEOM_SPHERE, size=np.array([radius, 0, 0]), pos=np.array(pos, dtype=np.float64), mat=np.eye(3).flatten(), rgba=np.array(rgba))
geom.category = mujoco.mjtCatBit.mjCAT_DECOR
viewer.user_scn.ngeom += 1
def add_line(viewer, p1, p2, width=0.01, rgba=[0, 0, 1, 1]):
MAX_GEOM_LIMIT = 10000
if viewer.user_scn.ngeom >= MAX_GEOM_LIMIT: return
p1, p2 = np.array(p1, dtype=np.float64), np.array(p2, dtype=np.float64)
mid = (p1 + p2) * 0.5; vec = p2 - p1; length = np.linalg.norm(vec)
if length < 1e-6: return
z_axis = vec / length
up = np.array([0, 0, 1]) if abs(z_axis[2]) < 0.9 else np.array([1, 0, 0])
x_axis = np.cross(up, z_axis); norm_x = np.linalg.norm(x_axis)
if norm_x < 1e-6: return
x_axis /= norm_x; y_axis = np.cross(z_axis, x_axis)
rot_mat = np.column_stack([x_axis, y_axis, z_axis]).flatten()
geom = viewer.user_scn.geoms[viewer.user_scn.ngeom]
mujoco.mjv_initGeom(geom, type=mujoco.mjtGeom.mjGEOM_CAPSULE, size=np.array([width, length * 0.5, 0]), pos=mid, mat=rot_mat, rgba=np.array(rgba))
geom.category = mujoco.mjtCatBit.mjCAT_DECOR
viewer.user_scn.ngeom += 1
if __name__ == "__main__":
main()
运行结果：
2.逆运动学转换脚本以及训练前的可视化验证
编写两个脚本，这两个脚本扮演了“翻译官”和“审核员”的核心角色。它们是能够成功启动 RL 训练的前提条件。简单来说，一个是把人动作变机器动作，一个是验证变完之后机器动作是否合理。
（1）motion_retargeting2.py (Translator)
这个脚本的核心任务是“跨物种动作映射”。它负责把人类动作数据转化为 G1 机器人能执行的动力学指令。
输入：来自 HybrIK 的原始人体动作数据 (.pk 文件，包含 24 个 SMPL 关节的 3D 坐标)。
核心工作流程：
解析与对齐：读取人体关节坐标，将其从“人类尺度”归一化到“G1 机器人尺度”。
运动学解算 (IK)：由于 G1 的骨架结构（29 个 DOF）与 SMPL（24 个关节点）并不完全一一对应，脚本通过逆运动学（Inverse Kinematics, minimize 函数）计算出：“如果人手的目标位置在这里，G1 的这 4 个手臂关节应该转多少角度？”
物理状态模拟 (FK)：通过 mujoco.mj_forward 计算出在这些角度下，机器人身体各部位的物理状态（速度、线速度、角速度）。
格式打包 (Export)：将上述计算结果打包成 g1_mjlab_tracking.npz。这里就是我们之前反复调整维度的关键点，确保生成的矩阵形状严格符合 mjlab 环境的 (N, 29) 要求。
#!/usr/bin/env python3
import os
import pickle
import numpy as np
import mujoco
from scipy.optimize import minimize
# ==============================================================================
# 1. 配置路径
# ==============================================================================
XML_PATH = '/home/labuser/unitree_ws/src/unitree_rl_mjlab/mjlab/asset_zoo/robots/unitree_g1/xmls/g1.xml'
PKL_PATH = '/home/labuser/unitree_ws/src/HybrIK/realsense_results/res_realsense.pk'
OUTPUT_PATH = '/home/labuser/unitree_ws/src/unitree_rl_mjlab/data/reference_motions/g1_mjlab_tracking.npz'
# SMPL 关节点定义
HUMAN_R_SHOULDER, HUMAN_R_ELBOW = 17, 19
RIGHT_ARM_JOINTS = ["right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint"]
# ==============================================================================
# 2. 核心 IK 求解器
# ==============================================================================
def solve_arm_ik(model, data, target_wrist_pos, target_elbow_pos, joint_names, site_id, elbow_body_id, default_arm_qpos):
qpos_indices = [model.jnt_qposadr[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)] for name in joint_names]
bounds = [model.jnt_range[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)] for name in joint_names]
init_q = data.qpos[qpos_indices].copy()
def loss_func(q_arm):
data.qpos[qpos_indices] = q_arm
mujoco.mj_forward(model, data)
wrist_loss = np.sum((data.site_xpos[site_id] - target_wrist_pos) ** 2)
elbow_loss = np.sum((data.xpos[elbow_body_id] - target_elbow_pos) ** 2)
reg_loss = 0.005 * np.sum((q_arm - default_arm_qpos) ** 2)
return wrist_loss + 1.2 * elbow_loss + reg_loss
res = minimize(loss_func, init_q, method='SLSQP', bounds=bounds, options={'maxiter': 50})
data.qpos[qpos_indices] = res.x
mujoco.mj_forward(model, data)
return res.x
# ==============================================================================
# 3. 主逻辑
# ==============================================================================
def main():
model = mujoco.MjModel.from_xml_path(XML_PATH)
data = mujoco.MjData(model)
with open(PKL_PATH, 'rb') as f:
hybrik_data = pickle.load(f)
pred_xyz = hybrik_data['pred_xyz_24_struct']
world_transl = hybrik_data['transl']
num_frames = pred_xyz.shape[0]
robot_qpos_trajectory = np.zeros((num_frames, model.nq))
mujoco.mj_resetData(model, data)
default_qpos = data.qpos.copy()
r_shoulder_bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "right_shoulder_pitch_link")
r_palm_sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "right_palm")
r_elbow_jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_elbow_joint")
r_elbow_bid = model.jnt_bodyid[r_elbow_jid]
r_jnt_idx = [model.jnt_qposadr[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, n)] for n in RIGHT_ARM_JOINTS]
# 容器
body_pos_w, body_quat_w, body_lin_vel_w, body_ang_vel_w = [], [], [], []
print("开始生成 mjlab 参考动作数据...")
for frame_idx in range(num_frames):
# 1. 躯干位置 (假设前 3 位是位置)
disp = world_transl[frame_idx] - world_transl[0]
data.qpos[0:3] = default_qpos[0:3] + np.array([-disp[2], disp[0], -disp[1]])
# 2. IK 求解
human_xyz = pred_xyz[frame_idx]
r_up_vec = np.array([-(human_xyz[HUMAN_R_ELBOW]-human_xyz[HUMAN_R_SHOULDER])[2], 0, 0])
solve_arm_ik(model, data, data.xpos[r_shoulder_bid] + r_up_vec*1.2, data.xpos[r_shoulder_bid] + r_up_vec, RIGHT_ARM_JOINTS, r_palm_sid, r_elbow_bid, default_qpos[r_jnt_idx])
robot_qpos_trajectory[frame_idx] = data.qpos.copy()
# 3. FK 数据提取
mujoco.mj_forward(model, data)
body_pos_w.append(data.xpos.copy())
body_quat_w.append(data.xquat.copy())
body_lin_vel_w.append(data.cvel[:, 3:].copy())
body_ang_vel_w.append(data.cvel[:, :3].copy())
# 4. 严格维度切片 (7: 对应浮动基座 7 自由度)
final_joint_pos = robot_qpos_trajectory[:, 7:]
robot_qvel = np.zeros_like(robot_qpos_trajectory)
robot_qvel[1:] = (robot_qpos_trajectory[1:] - robot_qpos_trajectory[:-1]) / (1.0/30.0)
final_joint_vel = robot_qvel[:, 7:]
np.savez(OUTPUT_PATH,
joint_pos=final_joint_pos,
joint_vel=final_joint_vel,
body_pos_w=np.stack(body_pos_w),
body_quat_w=np.stack(body_quat_w),
body_lin_vel_w=np.stack(body_lin_vel_w),
body_ang_vel_w=np.stack(body_ang_vel_w))
print(f"成功保存，输出文件: {OUTPUT_PATH}")
print(f"最终 joint_pos 维度: {final_joint_pos.shape}, joint_vel 维度: {final_joint_vel.shape}")
if __name__ == '__main__':
main()
（2） play_motion.py (The Viewer)
这个脚本的角色是“动作可视化验证”。在投入宝贵的 GPU 资源进行 RL 训练之前，必须先看一看生成出来的轨迹是不是“物理合理”的。
核心工作流程：
加载模型与轨迹：读取 g1.xml 场景和生成的 .npz 参考动作文件。
实时渲染：调用 mujoco.viewer 启动一个仿真窗口，按照时间序列把 .npz 里的 joint_pos 一帧一帧写给机器人。
视觉校准：你通过眼睛观察机器人执行动作时，是否存在：
自我碰撞 (Self-collision)：比如胳膊插进了身体。
关节扭曲：动作是否生硬，或者某些关节是否被强行拉扯到了极限值。
漂移：机器人是不是在按照你预期的轨迹走。
调试决策：如果看起来很诡异，需要回到 motion_retargeting.py 修改 IK 求解的权重（比如 reg_loss 或 elbow_loss），直到动作看起来自然为止。
#!/usr/bin/env python3
import os
import time
import numpy as np
import mujoco
import mujoco.viewer
# ==============================================================================
# 1. 路径配置（绝对路径，确保万无一失）
# ==============================================================================
XML_PATH = '/home/labuser/unitree_ws/src/unitree_rl_mjlab/mjlab/asset_zoo/robots/unitree_g1/xmls/g1.xml'
NPZ_PATH = '/home/labuser/unitree_ws/src/unitree_rl_mjlab/data/reference_motions/g1_upper_body.npz'
def main():
# 检查文件是否存在
if not os.path.exists(XML_PATH):
print(f"错误: 未找到 G1 模型文件 {XML_PATH}")
return
if not os.path.exists(NPZ_PATH):
print(f"错误: 未找到动作轨迹文件 {NPZ_PATH}，请先运行 IK 脚本生成它。")
return
# 2. 加载模型与数据
print("正在加载 MuJoCo 模型...")
model = mujoco.MjModel.from_xml_path(XML_PATH)
data = mujoco.MjData(model)
print("正在读取动作轨迹...")
motion_data = np.load(NPZ_PATH)
qpos_trajectory = motion_data['qpos']
num_frames = qpos_trajectory.shape[0]
print(f"成功加载动作为：{num_frames} 帧，每帧包含 {qpos_trajectory.shape[1]} 个自由度。")
print("正在启动 MuJoCo 可视化窗口（可在窗口内通过鼠标旋转/缩放视角）...")
# 3. 启动被动渲染器并循环播放
with mujoco.viewer.launch_passive(model, data) as viewer:
# 开启完美的默认视点（让机器人居中）
viewer.cam.distance = 2.0
viewer.cam.lookat = [0, 0, 0.7]
viewer.cam.elevation = -15
viewer.cam.azimuth = 135
print("\n>>> 开始循环播放动作。在终端按 Ctrl+C 或直接关闭渲染窗口退出。")
frame_idx = 0
fps = 30
frame_dt = 1.0 / fps
while viewer.is_running():
step_start = time.time()
# 将当前帧的关节角度写入机器人
data.qpos[:] = qpos_trajectory[frame_idx]
# 前向运动学计算更新骨骼位置
mujoco.mj_forward(model, data)
# 刷新渲染窗口
viewer.sync()
# 推进到下一帧（实现循环播放）
frame_idx = (frame_idx + 1) % num_frames
# 精准控制 30 FPS 帧率
elapsed = time.time() - step_start
if elapsed < frame_dt:
time.sleep(frame_dt - elapsed)
if __name__ == '__main__':
main()
运行结果：
下一步就是开始训练了，先尝试不更改奖励函数，看看训练效果
输出的动作包：/home/labuser/unitree_ws/src/unitree_rl_mjlab/data/reference_motions/g1_mjlab_tracking.npz
python3 scripts/train.py Mjlab-Tracking-Flat-Unitree-G1 --motion-file=./data/reference_motions/g1_mjlab_tracking.npz --env.scene.num_envs=64
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
5
0- 0
已为社区贡献9条内容
所有评论(0)