机器人舞蹈动作：采集 - 训练 - 生成完整代码框架
这是一份框架贴合宇树机器人（G1 43 关节）的硬件特性，整合了动作采集、MuJoCo 仿真、强化学习训练、动作生成与适配全流程，包含主要代码与可落地的核心逻辑，可直接基于此扩展实际项目。
一、整体架构说明
- 核心流程：人类动作采集 → 数据预处理与标准化 → 机器人运动学重映射 → MuJoCo仿真训练 → 强化学习策略优化 → 动作生成与实时执行
- 适配宇树机器人：关节自由度（G1 43 关节）、力矩限制、平衡特性
- 技术栈：Python + OpenCV（姿态估计） + PyTorch（RL / PPO） + MuJoCo（物理仿真） + 逆运动学（IK）求解
二、模块 1：动作采集（单目相机捕获人类舞蹈动作）
输入为视觉姿态估计（低成本-单目相机 ）或惯性动捕（高精度，对应宇树官方动捕方案-IMU）两种方式，输出标准化关节数据。
bash
import cv2
import mediapipe as mp
import numpy as np
from scipy import signal
import time
# 配置参数（贴合宇树动作采集要求）
JOINT_NUM = 33 # 人体关键关节数（与机器人关节映射）
FRAME_RATE = 30 # 采集帧率（宇树机器人支持最高100Hz，舞蹈场景30Hz足够）
AUDIO_BEAT_THRESHOLD = 0.8 # 音乐节拍检测阈值
COORDINATE_OFFSET = np.array([0, 0, 0]) # 坐标系偏移（对齐机器人基坐标）
class HumanMotionCapture:
def __init__(self, capture_type="vision"):
"""
初始化动作采集器
:param capture_type: "vision"（视觉，低成本） / "imu"（惯性动捕，高精度，宇树推荐）
"""
self.capture_type = capture_type
self.motion_data = [] # 存储采集到的关节数据 (frame_num, joint_num, 3)
self.beat_timestamps = [] # 音乐节拍时间戳
self.is_recording = False
# 视觉姿态估计初始化（MediaPipe，低成本替代方案）
if self.capture_type == "vision":
self.mp_pose = mp.solutions.pose
self.pose = self.mp_pose.Pose(static_image_mode=False, min_detection_confidence=0.7)
self.mp_drawing = mp.solutions.drawing_utils
self.cap = cv2.VideoCapture(0) # 摄像头采集
def detect_audio_beat(self, audio_path):
"""
解析音乐节拍（宇树舞蹈卡点核心），生成时间戳
:param audio_path: 音乐文件路径
:return: 节拍时间戳列表
"""
# 简化实现：实际可使用librosa库提取频谱与节拍
import librosa
y, sr = librosa.load(audio_path)
tempo, beat_frames = librosa.beat.beat_track(y=y, sr=sr)
self.beat_timestamps = librosa.frames_to_time(beat_frames, sr=sr)
print(f"检测到音乐节拍数：{len(self.beat_timestamps)}，速度：{tempo} BPM")
return self.beat_timestamps
def capture_single_frame(self):
"""采集单帧人体关节数据"""
if self.capture_type == "vision":
ret, frame = self.cap.read()
if not ret:
return None
# 颜色空间转换
image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = self.pose.process(image)
if not results.pose_landmarks:
return None
# 提取关节坐标
joint_coords = []
for landmark in results.pose_landmarks.landmark:
x = landmark.x - 0.5 # 标准化到[-0.5, 0.5]
y = landmark.y - 0.5
z = landmark.z - 0.5 # 深度信息（MediaPipe输出相对值）
joint_coords.append([x, y, z])
# 绘制姿态（可视化）
self.mp_drawing.draw_landmarks(frame, results.pose_landmarks, self.mp_pose.POSE_CONNECTIONS)
cv2.imshow("Human Motion Capture", frame)
cv2.waitKey(1)
return np.array(joint_coords)
elif self.capture_type == "imu":
# 宇树惯性动捕设备（如VDSuit Full）数据读取（伪代码，需对接硬件SDK）
imu_data = self.read_imu_hardware() # 硬件SDK接口
joint_angles = self.imu_to_joint_angles(imu_data) # IMU数据转关节角度
return joint_angles
else:
return None
def read_imu_hardware(self):
"""读取IMU硬件数据（伪代码，需对接宇树/第三方动捕SDK）"""
# 模拟IMU数据输出 (joint_num, 3) （角度：x,y,z轴）
return np.random.rand(JOINT_NUM, 3) * np.pi # 弧度制
def imu_to_joint_angles(self, imu_data):
"""IMU数据转人体关节角度（宇树高精度采集核心逻辑）"""
# 实际需结合运动学模型，去除IMU漂移噪声
imu_data = signal.medfilt(imu_data, kernel_size=3) # 中值滤波去噪
return imu_data
def start_recording(self):
"""开始采集动作"""
self.is_recording = True
self.motion_data = []
start_time = time.time()
print("动作采集开始...")
while self.is_recording:
frame_data = self.capture_single_frame()
if frame_data is not None:
self.motion_data.append(frame_data)
# 控制采集帧率
elapsed_time = time.time() - start_time
target_frame_time = len(self.motion_data) / FRAME_RATE
if elapsed_time < target_frame_time:
time.sleep(target_frame_time - elapsed_time)
# 按ESC停止采集
if cv2.waitKey(1) & 0xFF == 27:
self.stop_recording()
def stop_recording(self):
"""停止采集动作"""
self.is_recording = False
self.motion_data = np.array(self.motion_data)
print(f"动作采集结束，共采集 {len(self.motion_data)} 帧数据")
if self.capture_type == "vision":
self.cap.release()
cv2.destroyAllWindows()
# 保存采集数据
np.save("human_motion_data.npy", self.motion_data)
np.save("beat_timestamps.npy", self.beat_timestamps)
# 采集示例
if __name__ == "__main__":
capture = HumanMotionCapture(capture_type="vision")
capture.detect_audio_beat("dance_music.wav") # 先解析音乐节拍
capture.start_recording() # 开始采集动作三、模块 2：数据预处理与机器人运动学重映射
将人类动作数据转换为宇树机器人可执行的关节角度，核心是逆运动学（IK）求解与关节约束裁剪（贴合机器人硬件限制），适应一般大开大合的动作就可以了。若动作是强风格、强变换和多协同(腿、脚、腰等)的就要走QP优化的方法(推荐GMR或者Playground)。
bash
import numpy as np
from scipy.optimize import minimize
# 宇树机器人参数（以G1为例，可替换为A1/B1参数）
ROBOT_JOINT_NUM = 43 # G1机器人关节数
ROBOT_JOINT_LIMITS = {
"hip": (-np.pi/3, np.pi/3), # 髋关节角度限制
"knee": (0, np.pi/2), # 膝关节角度限制
"arm": (-np.pi/2, np.pi/2), # 手臂关节角度限制
# 其他关节限制...
}
ROBOT_BASE_HEIGHT = 0.8 # 机器人基础高度（米）
class MotionRemapper:
def __init__(self):
self.human_motion = np.load("human_motion_data.npy") # 加载人类动作数据
self.beat_timestamps = np.load("beat_timestamps.npy") # 加载节拍时间戳
self.robot_motion = [] # 存储映射后的机器人关节数据
def human_to_robot_coordinate(self, human_joint):
"""
人类坐标系 → 机器人坐标系转换（宇树机器人基坐标对齐）
:param human_joint: 单个人体关节坐标 (3,)
:return: 机器人坐标系下关节坐标 (3,)
"""
# 1. 坐标系旋转（人类直立坐标系 → 机器人坐标系）
rotation_matrix = np.array([[1, 0, 0],
[0, -1, 0],
[0, 0, -1]]) # 视实际情况调整
# 2. 平移与缩放（适配机器人身高）
scaled_joint = human_joint * 0.8 # 缩放到机器人尺寸
robot_joint = np.dot(rotation_matrix, scaled_joint) + COORDINATE_OFFSET
# 3. 高度补偿（确保机器人足底不悬空）
robot_joint[2] += ROBOT_BASE_HEIGHT
return robot_joint
def inverse_kinematics(self, target_end_effector_pos, robot_joint_init):
"""
逆运动学（IK）求解：目标末端执行器位置 → 机器人关节角度
（宇树机器人动作适配核心，确保动作不超限、可执行）
:param target_end_effector_pos: 末端执行器目标位置 (3,)
:param robot_joint_init: 关节角度初始值 (ROBOT_JOINT_NUM,)
:return: 可行的机器人关节角度 (ROBOT_JOINT_NUM,)
"""
def cost_function(robot_joints):
"""代价函数：末端执行器实际位置与目标位置的误差"""
# 正运动学求解：关节角度 → 末端执行器位置（宇树机器人正运动学模型）
end_effector_pos = self.forward_kinematics(robot_joints)
# 计算位置误差
error = np.linalg.norm(end_effector_pos - target_end_effector_pos)
return error
def joint_constraint(robot_joints):
"""关节角度约束：确保不超出机器人硬件限制"""
constraints = []
# 髋关节约束（示例，可扩展其他关节）
hip_joints = robot_joints[0:6]
for hip_joint in hip_joints:
if not (ROBOT_JOINT_LIMITS["hip"][0] <= hip_joint <= ROBOT_JOINT_LIMITS["hip"][1]):
constraints.append(1e6) # 超出约束则惩罚
# 膝关节约束
knee_joints = robot_joints[6:12]
for knee_joint in knee_joints:
if not (ROBOT_JOINT_LIMITS["knee"][0] <= knee_joint <= ROBOT_JOINT_LIMITS["knee"][1]):
constraints.append(1e6)
return sum(constraints)
# 最小化代价函数，求解可行关节角度
result = minimize(
fun=cost_function,
x0=robot_joint_init,
constraints={"type": "ineq", "fun": lambda x: -joint_constraint(x)},
method="SLSQP",
tol=1e-6
)
if result.success:
return result.x
else:
print("IK求解失败，使用初始关节角度")
return robot_joint_init
def forward_kinematics(self, robot_joints):
"""
正运动学求解：机器人关节角度 → 末端执行器位置（宇树机器人官方模型）
（伪代码，实际需使用宇树机器人DH参数或URDF模型）
:param robot_joints: 机器人关节角度 (ROBOT_JOINT_NUM,)
:return: 末端执行器位置 (3,)
"""
# 模拟正运动学输出（实际需对接宇树机器人运动学SDK）
return np.array([
np.sum(robot_joints[0:8]) * 0.1,
np.sum(robot_joints[8:16]) * 0.1,
np.sum(robot_joints[16:24]) * 0.1 + ROBOT_BASE_HEIGHT
])
def motion_remap(self):
"""批量将人类动作映射为机器人动作"""
robot_joint_init = np.zeros(ROBOT_JOINT_NUM) # 初始关节角度（直立姿态）
for human_frame in self.human_motion:
# 1. 人类关节坐标 → 机器人关节坐标
robot_joint_coords = []
for human_joint in human_frame:
robot_joint_coord = self.human_to_robot_coordinate(human_joint)
robot_joint_coords.append(robot_joint_coord)
robot_joint_coords = np.array(robot_joint_coords)
# 2. 提取末端执行器（手/脚）目标位置，执行IK求解
end_effector_idx = [28, 29, 30, 31, 32] # 手脚末端关节索引
target_pos = robot_joint_coords[end_effector_idx].mean(axis=0) # 平均目标位置
robot_joints = self.inverse_kinematics(target_pos, robot_joint_init)
# 3. 节拍卡点调整：对齐音乐时间戳，调整动作速度
robot_joints = self.adjust_beat(robot_joints)
# 4. 保存映射后的关节角度
self.robot_motion.append(robot_joints)
robot_joint_init = robot_joints # 更新初始值，保证动作平滑
self.robot_motion = np.array(self.robot_motion)
print(f"动作映射完成，机器人动作数据形状：{self.robot_motion.shape}")
np.save("robot_motion_raw.npy", self.robot_motion)
return self.robot_motion
def adjust_beat(self, robot_joints):
"""调整机器人关节角度，确保动作与音乐节拍卡点（宇树舞蹈核心优化）"""
# 简化实现：实际需根据节拍时间戳调整动作幅度与时序
current_frame = len(self.robot_motion)
current_time = current_frame / FRAME_RATE
# 检查是否处于节拍时刻附近
for beat_time in self.beat_timestamps:
if abs(current_time - beat_time) < 0.05: # 50ms内视为节拍时刻
# 增大关节动作幅度，实现卡点效果
robot_joints = robot_joints * 1.2
break
return robot_joints
# 映射示例
if __name__ == "__main__":
remapper = MotionRemapper()
remapper.motion_remap()四、模块 3：MuJoCo 仿真环境搭建（机器人动作预演simu2simu）
基于 MuJoCo 构建机器人数字孪生模型，用于动作可行性验证与强化学习训练，避免真实机器人损坏，再者就是走一下"牛顿"的审批。
bash
import mujoco
import mujoco_viewer
import numpy as np
import time
class UnitreeMuJoCoSim:
def __init__(self, urdf_path="unitree_g1.urdf"):
"""
初始化宇树机器人MuJoCo仿真环境
:param urdf_path: 宇树机器人URDF模型路径（可从官方获取）
"""
# 加载MuJoCo模型（URDF/XML）
self.model = mujoco.MjModel.from_xml_path(urdf_path)
self.data = mujoco.MjData(self.model)
# 初始化仿真查看器
self.viewer = mujoco_viewer.MujocoViewer(self.model, self.data)
# 加载机器人原始动作数据
self.robot_motion = np.load("robot_motion_raw.npy")
self.current_frame = 0
self.sim_time = 0
self.sim_step = 0.001 # 仿真步长（1ms，保证精度）
def reset_sim(self):
"""重置仿真环境（机器人回到直立姿态）"""
mujoco.mj_resetData(self.model, self.data)
# 设置初始关节角度
self.data.qpos[:ROBOT_JOINT_NUM] = np.zeros(ROBOT_JOINT_NUM)
self.current_frame = 0
self.sim_time = 0
print("仿真环境已重置")
def set_robot_joints(self, robot_joints):
"""设置机器人关节角度（跟踪目标动作）"""
# 确保关节角度不超出仿真模型限制
for i in range(ROBOT_JOINT_NUM):
if robot_joints[i] < self.model.jnt_range[i][0]:
robot_joints[i] = self.model.jnt_range[i][0]
elif robot_joints[i] > self.model.jnt_range[i][1]:
robot_joints[i] = self.model.jnt_range[i][1]
# 设置关节位置与速度
self.data.qpos[:ROBOT_JOINT_NUM] = robot_joints
self.data.qvel[:ROBOT_JOINT_NUM] = np.zeros(ROBOT_JOINT_NUM) # 初始速度为0
def sim_step(self):
"""单步仿真"""
if self.current_frame >= len(self.robot_motion):
print("动作序列已执行完毕")
return False
# 1. 获取当前帧机器人关节角度
target_joints = self.robot_motion[self.current_frame]
# 2. 设置机器人关节角度
self.set_robot_joints(target_joints)
# 3. 执行仿真步
mujoco.mj_step(self.model, self.data)
# 4. 更新查看器
self.viewer.render()
# 5. 更新帧索引与仿真时间
self.current_frame += 1
self.sim_time += self.sim_step
# 6. 控制仿真帧率与动作帧率同步
time.sleep(self.sim_step)
return True
def run_sim(self):
"""运行完整动作仿真（预演）"""
self.reset_sim()
print("开始MuJoCo仿真预演...")
while self.sim_step():
pass
self.close_sim()
def close_sim(self):
"""关闭仿真查看器"""
self.viewer.close()
print("仿真环境已关闭")
# 仿真示例
if __name__ == "__main__":
sim = UnitreeMuJoCoSim(urdf_path="unitree_g1.xml") # 替换为实际URDF/XML路径
sim.run_sim()五、模块 4：强化学习(RL，PPO)训练（动作优化，提升稳定性）
基于PPO 算法（适合机器人控制任务），在 MuJoCo 仿真环境中优化机器人舞蹈动作，提升平衡稳定性与动作流畅度，贴合宇树机器人强化学习训练方案。这一步是百花齐放，各有所长，无论那条路径细化调参和适配修改都能达到相同的效果。
bash
import torch
import torch.nn as nn
import torch.optim as optim
import numpy as np
from collections import deque
import random
# 设备配置（GPU优先）
DEVICE = torch.device("cuda" if torch.cuda.is_available() else "cpu")
# PPO算法参数
GAMMA = 0.99 # 折扣因子
LAMDA = 0.95 # GAE参数
CLIP_EPS = 0.2 # 裁剪系数
LEARNING_RATE = 3e-4
BATCH_SIZE = 64
EPISODE_NUM = 1000 # 训练轮数
class ActorCritic(nn.Module):
"""PPO Actor-Critic网络（策略网络+价值网络）"""
def __init__(self, state_dim, action_dim, action_bound):
super(ActorCritic, self).__init__()
self.action_bound = action_bound # 机器人关节动作边界
# Actor网络：状态 → 动作分布
self.actor = nn.Sequential(
nn.Linear(state_dim, 256),
nn.ReLU(),
nn.Linear(256, 128),
nn.ReLU(),
nn.Linear(128, action_dim),
nn.Tanh() # 输出范围[-1,1]，后续缩放至动作边界
)
# 动作方差（可学习）
self.log_std = nn.Parameter(torch.zeros(1, action_dim))
# Critic网络：状态 → 状态价值
self.critic = nn.Sequential(
nn.Linear(state_dim, 256),
nn.ReLU(),
nn.Linear(256, 128),
nn.ReLU(),
nn.Linear(128, 1)
)
def get_action(self, state):
"""给定状态，输出动作与对数概率"""
state = torch.tensor(state, dtype=torch.float32).to(DEVICE)
mu = self.actor(state)
mu = mu * self.action_bound # 缩放至动作边界
std = torch.exp(self.log_std)
# 正态分布采样动作
dist = torch.distributions.Normal(mu, std)
action = dist.sample()
log_prob = dist.log_prob(action).sum(dim=-1)
return action.cpu().detach().numpy(), log_prob.cpu().detach().numpy()
def evaluate(self, state, action):
"""评估动作的对数概率与状态价值"""
state = torch.tensor(state, dtype=torch.float32).to(DEVICE)
action = torch.tensor(action, dtype=torch.float32).to(DEVICE)
mu = self.actor(state)
mu = mu * self.action_bound
std = torch.exp(self.log_std)
dist = torch.distributions.Normal(mu, std)
log_prob = dist.log_prob(action).sum(dim=-1)
value = self.critic(state).squeeze()
return log_prob, value
class PPO Trainer:
def __init__(self, state_dim, action_dim, action_bound):
self.ac_net = ActorCritic(state_dim, action_dim, action_bound).to(DEVICE)
self.optimizer = optim.Adam(self.ac_net.parameters(), lr=LEARNING_RATE)
self.memory = deque(maxlen=10000) # 经验缓冲区
def store_experience(self, state, action, reward, next_state, done, log_prob):
"""存储经验"""
self.memory.append((state, action, reward, next_state, done, log_prob))
def compute_gae(self, rewards, values, dones):
"""计算广义优势估计（GAE）"""
advantages = np.zeros_like(rewards)
advantage = 0
for t in reversed(range(len(rewards)-1)):
delta = rewards[t] + GAMMA * values[t+1] * (1 - dones[t]) - values[t]
advantage = delta + GAMMA * LAMDA * (1 - dones[t]) * advantage
advantages[t] = advantage
returns = advantages + values
# 标准化优势
advantages = (advantages - np.mean(advantages)) / (np.std(advantages) + 1e-8)
return advantages, returns
def update(self):
"""PPO网络更新"""
# 提取经验数据
batch = random.sample(self.memory, BATCH_SIZE)
states, actions, rewards, next_states, dones, old_log_probs = zip(*batch)
states = np.array(states)
actions = np.array(actions)
rewards = np.array(rewards)
dones = np.array(dones)
old_log_probs = np.array(old_log_probs)
# 计算状态价值
values = []
for state in states:
_, value = self.ac_net.evaluate(state, np.zeros_like(actions[0]))
values.append(value.cpu().detach().numpy())
values = np.array(values)
next_values = []
for next_state in next_states:
_, next_value = self.ac_net.evaluate(next_state, np.zeros_like(actions[0]))
next_values.append(next_value.cpu().detach().numpy())
next_values = np.array(next_values)
values = np.concatenate([values, next_values[-1:]])
# 计算GAE与回报
advantages, returns = self.compute_gae(rewards, values, dones)
advantages = torch.tensor(advantages, dtype=torch.float32).to(DEVICE)
returns = torch.tensor(returns, dtype=torch.float32).to(DEVICE)
old_log_probs = torch.tensor(old_log_probs, dtype=torch.float32).to(DEVICE)
# 网络更新
for _ in range(10): # 多次更新
log_probs, current_values = self.ac_net.evaluate(states, actions)
# 裁剪损失（Actor）
ratio = torch.exp(log_probs - old_log_probs)
surr1 = ratio * advantages
surr2 = torch.clamp(ratio, 1 - CLIP_EPS, 1 + CLIP_EPS) * advantages
actor_loss = -torch.min(surr1, surr2).mean()
# 价值损失（Critic）
critic_loss = nn.MSELoss()(current_values, returns[:-1])
# 总损失
total_loss = actor_loss + 0.5 * critic_loss
# 反向传播
self.optimizer.zero_grad()
total_loss.backward()
self.optimizer.step()
def train(self, sim_env):
"""
在MuJoCo仿真环境中训练
:param sim_env: 宇树机器人MuJoCo仿真环境
"""
state_dim = sim_env.model.nq + sim_env.model.nv # 状态维度：关节位置+关节速度
action_dim = ROBOT_JOINT_NUM # 动作维度：机器人关节数
action_bound = np.pi / 3 # 动作边界（关节角度最大变化量）
for episode in range(EPISODE_NUM):
sim_env.reset_sim()
state = np.concatenate([sim_env.data.qpos, sim_env.data.qvel]) # 初始状态
episode_reward = 0
done = False
step = 0
while not done:
# 1. 获取动作
action, log_prob = self.ac_net.get_action(state)
# 2. 执行动作（设置机器人关节角度）
target_joints = sim_env.data.qpos[:ROBOT_JOINT_NUM] + action
sim_env.set_robot_joints(target_joints)
sim_env.sim_step()
# 3. 获取下一状态与奖励
next_state = np.concatenate([sim_env.data.qpos, sim_env.data.qvel])
# 奖励函数设计（宇树机器人舞蹈优化核心）
reward = self.calculate_reward(sim_env)
# 4. 判断是否结束
done = not sim_env.sim_step() or self.check_fall(sim_env)
# 5. 存储经验
self.store_experience(state, action, reward, next_state, done, log_prob)
# 6. 更新状态与累计奖励
state = next_state
episode_reward += reward
step += 1
# 经验足够时更新网络
if len(self.memory) >= BATCH_SIZE:
self.update()
# 打印训练信息
if (episode + 1) % 10 == 0:
print(f"Episode: {episode+1}/{EPISODE_NUM}, Reward: {episode_reward:.2f}, Step: {step}")
# 保存模型
torch.save(self.ac_net.state_dict(), "unitree_ppo_model.pth")
def calculate_reward(self, sim_env):
"""
奖励函数设计（关键：提升机器人平衡与动作流畅度）
:param sim_env: MuJoCo仿真环境
:return: 奖励值
"""
reward = 0
# 1. 平衡奖励：机器人重心高度（越高奖励越高，防止摔倒）
com_pos = sim_env.data.subtree_com[0] # 重心位置
com_height = com_pos[2]
reward += com_height * 10
# 2. 动作跟踪奖励：与目标舞蹈动作的误差（误差越小奖励越高）
target_joints = sim_env.robot_motion[min(sim_env.current_frame, len(sim_env.robot_motion)-1)]
current_joints = sim_env.data.qpos[:ROBOT_JOINT_NUM]
joint_error = np.linalg.norm(current_joints - target_joints)
reward -= joint_error * 5
# 3. 动作平滑奖励：关节速度（速度越小越平滑，奖励越高）
joint_vel = sim_env.data.qvel[:ROBOT_JOINT_NUM]
vel_norm = np.linalg.norm(joint_vel)
reward -= vel_norm * 0.1
# 4. 节拍卡点奖励：处于节拍时刻时额外奖励
current_time = sim_env.sim_time
for beat_time in sim_env.beat_timestamps:
if abs(current_time - beat_time) < 0.05:
reward += 20
break
return reward
def check_fall(self, sim_env):
"""判断机器人是否摔倒（重心高度过低则视为摔倒）"""
com_height = sim_env.data.subtree_com[0][2]
return com_height < ROBOT_BASE_HEIGHT * 0.5 # 重心高度低于基础高度的50%
# 训练示例
if __name__ == "__main__":
# 1. 初始化仿真环境
sim_env = UnitreeMuJoCoSim(urdf_path="unitree_g1.xml")
# 2. 初始化PPO训练器
state_dim = sim_env.model.nq + sim_env.model.nv
action_dim = ROBOT_JOINT_NUM
action_bound = np.pi / 3
ppo_trainer = PPO Trainer(state_dim, action_dim, action_bound)
# 3. 开始训练
ppo_trainer.train(sim_env)六、模块 5：动作生成与实时执行(simu2Real)
加载训练好的模型，生成流畅的舞蹈动作，并适配宇树机器人硬件进行实时执行。simu2simu和simu2Real是逐渐优化循环的过程，大多数都不是一遍过的，需要循环多次。
bash
import torch
import numpy as np
import time
class MotionGenerator:
def __init__(self, model_path="unitree_ppo_model.pth"):
"""初始化动作生成器"""
self.model_path = model_path
self.state_dim = ROBOT_JOINT_NUM * 2 # 状态维度：关节位置+关节速度
self.action_dim = ROBOT_JOINT_NUM
self.action_bound = np.pi / 3
# 加载PPO模型
self.ac_net = ActorCritic(self.state_dim, self.action_dim, self.action_bound).to(DEVICE)
self.ac_net.load_state_dict(torch.load(self.model_path, map_location=DEVICE))
self.ac_net.eval() # 评估模式
# 加载基础动作与节拍数据
self.robot_motion_base = np.load("robot_motion_raw.npy")
self.beat_timestamps = np.load("beat_timestamps.npy")
def generate_motion(self, length=1000):
"""
生成指定长度的舞蹈动作
:param length: 动作帧数
:return: 生成的机器人关节动作 (length, ROBOT_JOINT_NUM)
"""
generated_motion = []
# 初始状态（直立姿态）
current_joints = np.zeros(ROBOT_JOINT_NUM)
current_vel = np.zeros(ROBOT_JOINT_NUM)
for _ in range(length):
# 1. 构建状态
state = np.concatenate([current_joints, current_vel])
# 2. 模型预测动作
action, _ = self.ac_net.get_action(state)
# 3. 更新关节角度
next_joints = current_joints + action
# 4. 节拍卡点优化
next_joints = self.adjust_beat(next_joints, len(generated_motion))
# 5. 保存动作
generated_motion.append(next_joints)
# 6. 更新当前状态
current_joints = next_joints
current_vel = (next_joints - current_joints) / (1 / FRAME_RATE) # 计算关节速度
generated_motion = np.array(generated_motion)
print(f"动作生成完成，动作长度：{len(generated_motion)} 帧")
np.save("unitree_dance_motion.npy", generated_motion)
return generated_motion
def adjust_beat(self, joints, frame_idx):
"""节拍卡点优化（与模块2一致，确保生成动作卡点）"""
current_time = frame_idx / FRAME_RATE
for beat_time in self.beat_timestamps:
if abs(current_time - beat_time) < 0.05:
joints = joints * 1.2
break
return joints
def send_to_robot(self, motion_data, robot_ip="192.168.1.10"):
"""
将动作发送到宇树机器人（实时执行）
（伪代码，需对接宇树机器人官方SDK，如Unitree SDK）
:param motion_data: 机器人关节动作数据
:param robot_ip: 宇树机器人IP地址
"""
# 1. 连接机器人（宇树SDK接口）
# unitree_sdk.connect(robot_ip)
print(f"已连接宇树机器人：{robot_ip}")
# 2. 实时发送关节数据
for joints in motion_data:
# unitree_sdk.set_joint_angles(joints) # 宇树SDK设置关节角度
print(f"发送关节角度：{joints[:3]}...（省略其他关节）")
time.sleep(1 / FRAME_RATE) # 按帧率发送
# 3. 断开连接
# unitree_sdk.disconnect()
print("动作执行完毕，已断开机器人连接")
# 动作生成与执行示例
if __name__ == "__main__":
generator = MotionGenerator()
# 1. 生成舞蹈动作
dance_motion = generator.generate_motion(length=300) # 10秒动作（30帧/秒）
# 2. 发送到宇树机器人执行
generator.send_to_robot(dance_motion, robot_ip="192.168.1.10")七、结束语
核心流程闭环：人类动作采集→运动学重映射→MuJoCo仿真→PPO强化学习优化→动作生成与硬件执行，完全贴合宇树机器人技术方案。- 关键亮点：
- 支持音乐节拍卡点，实现舞蹈同步效果；
- 考虑机器人关节约束与平衡特性，避免动作超限与摔倒；
- MuJoCo 仿真预演降低真实机器人损坏风险，PPO 算法提升动作稳定性；
- 落地提示：
- 需替换宇树机器人 URDF/XML 模型与官方 SDK 接口；
- 可根据实际机器人型号（A1/B1/G1）调整关节数与约束参数；
- 强化学习训练需根据硬件性能调整 EPISODE_NUM 与 BATCH_SIZE。
最后的话：对于简单的动作复现应该是不成问题了。如若你有一套亮眼的舞\武术要整下来，表演一下，顺便挣点元子。那么你首先要做的是把上边的统统忘掉，组建个靠谱的团队，细细打磨，紧盯细节，多尝试方法，肯定是能实现的。