IndexError: index 11 is out of bounds for axis 0 with size 11修正代码#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
完整的人形机器人仿真系统 - 包含 MuJoCoRenderer 定义和增强功能
"""
import sys
import os
import time
import json
import math
import random
import numpy as np
import glfw
import mujoco
import imageio
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as R
from collections import deque
from typing import Dict, List, Tuple, Optional, Callable
# ======================== MuJoCo 渲染器定义 ========================
class MuJoCoRenderer:
def __init__(self, model, title="Humanoid Simulation", width=1200, height=800):
self.model = model
self.data = mujoco.MjData(model)
self.width = width
self.height = height
self.title = title
# 初始化 GLFW
if not glfw.init():
raise RuntimeError("Could not initialize GLFW")
# 创建窗口
self.window = glfw.create_window(width, height, title, None, None)
if not self.window:
glfw.terminate()
raise RuntimeError("Could not create GLFW window")
glfw.make_context_current(self.window)
glfw.swap_interval(1) # 开启垂直同步
# 初始化 MuJoCo 渲染组件
self.cam = mujoco.MjvCamera()
self.opt = mujoco.MjvOption()
self.scene = mujoco.MjvScene(model, maxgeom=1000)
self.context = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150)
# 设置相机初始参数
self.cam.distance = 5.0
self.cam.elevation = -20
self.cam.azimuth = 90
# 注册回调函数
glfw.set_key_callback(self.window, self.keyboard_callback)
glfw.set_cursor_pos_callback(self.window, self.mouse_move_callback)
glfw.set_mouse_button_callback(self.window, self.mouse_button_callback)
glfw.set_scroll_callback(self.window, self.scroll_callback)
glfw.set_framebuffer_size_callback(self.window, self.resize_callback)
# 帧缓冲区
self.framebuffer = np.zeros((height, width, 3), dtype=np.uint8)
# 状态变量
self.button_left = False
self.button_middle = False
self.button_right = False
self.last_x = 0
self.last_y = 0
self.paused = False
self.camera_mode = 0
self.move_forward = False
self.move_backward = False
self.move_left = False
self.move_right = False
print("✅ MuJoCo 渲染器初始化完成")
def keyboard_callback(self, window, key, scancode, action, mods):
"""键盘回调函数"""
if action != glfw.PRESS:
return
# ESC 键退出
if key == glfw.KEY_ESCAPE:
glfw.set_window_should_close(window, True)
# 空格键暂停/继续
elif key == glfw.KEY_SPACE:
self.paused = not self.paused
# 方向键控制机器人
elif key == glfw.KEY_UP:
self.move_forward = True
elif key == glfw.KEY_DOWN:
self.move_backward = True
elif key == glfw.KEY_LEFT:
self.move_left = True
elif key == glfw.KEY_RIGHT:
self.move_right = True
# R 键重置
elif key == glfw.KEY_R:
self.reset_simulation()
# C 键切换相机模式
elif key == glfw.KEY_C:
self.camera_mode = (self.camera_mode + 1) % 3
self.update_camera()
def mouse_move_callback(self, window, xpos, ypos):
"""鼠标移动回调函数"""
dx = xpos - self.last_x
dy = ypos - self.last_y
self.last_x = xpos
self.last_y = ypos
# 没有按钮按下时不操作
if not (self.button_left or self.button_middle or self.button_right):
return
# 获取当前窗口大小
width, height = glfw.get_window_size(window)
# 鼠标左键:旋转视角
if self.button_left:
self.cam.azimuth -= dx * 0.5
self.cam.elevation += dy * 0.5
self.cam.elevation = max(-90, min(90, self.cam.elevation))
# 鼠标右键:平移视角
elif self.button_right:
self.cam.track(-dx * 0.01, dy * 0.01)
# 鼠标中键:缩放
elif self.button_middle:
self.cam.distance *= (1.0 + 0.05 * dy)
self.cam.distance = max(0.1, min(50, self.cam.distance))
def mouse_button_callback(self, window, button, action, mods):
"""鼠标按钮回调函数"""
self.button_left = (button == glfw.MOUSE_BUTTON_LEFT and action == glfw.PRESS)
self.button_middle = (button == glfw.MOUSE_BUTTON_MIDDLE and action == glfw.PRESS)
self.button_right = (button == glfw.MOUSE_BUTTON_RIGHT and action == glfw.PRESS)
# 更新鼠标位置
self.last_x, self.last_y = glfw.get_cursor_pos(window)
def scroll_callback(self, window, xoffset, yoffset):
"""滚轮回调函数"""
self.cam.distance *= (1.0 - 0.1 * yoffset)
self.cam.distance = max(0.1, min(50, self.cam.distance))
def resize_callback(self, window, width, height):
"""窗口大小调整回调函数"""
self.width = width
self.height = height
self.framebuffer = np.zeros((height, width, 3), dtype=np.uint8)
glfw.make_context_current(window)
mujoco.mjr_resizeOffscreenBuffer(self.context, width, height)
def update_camera(self):
"""更新相机视角"""
if self.camera_mode == 0: # 默认视角
self.cam.distance = 5.0
self.cam.elevation = -20
self.cam.azimuth = 90
elif self.camera_mode == 1: # 跟随视角
self.cam.distance = 3.0
self.cam.elevation = -10
self.cam.azimuth = 45
elif self.camera_mode == 2: # 第一人称视角
self.cam.distance = 1.5
self.cam.elevation = 0
self.cam.azimuth = 0
def reset_simulation(self):
"""重置仿真"""
mujoco.mj_resetData(self.model, self.data)
self.data.qpos[0:3] = [0, 0, 1.0] # 初始位置
self.data.qpos[3:7] = [1, 0, 0, 0] # 初始朝向(四元数)
mujoco.mj_forward(self.model, self.data)
def render(self):
"""渲染一帧"""
# 更新场景
mujoco.mjv_updateScene(
self.model, self.data, self.opt, None, self.cam,
mujoco.mjtCatBit.mjCAT_ALL, self.scene)
# 设置视口
viewport = mujoco.MjrRect(0, 0, self.width, self.height)
# 渲染到屏幕
mujoco.mjr_render(viewport, self.scene, self.context)
# 交换缓冲区
glfw.swap_buffers(self.window)
# 处理事件
glfw.poll_events()
def capture_frame(self):
"""捕获当前帧作为图像"""
# 确保帧缓冲区大小正确
if self.framebuffer.shape[0] != self.height or self.framebuffer.shape[1] != self.width:
self.framebuffer = np.zeros((self.height, self.width, 3), dtype=np.uint8)
# 设置视口
viewport = mujoco.MjrRect(0, 0, self.width, self.height)
# 读取像素
mujoco.mjr_readPixels(self.framebuffer, None, viewport, self.context)
# 图像需要垂直翻转
return np.flipud(self.framebuffer)
def close(self):
"""关闭渲染器"""
glfw.terminate()
# ======================== 高级控制器定义 ========================
class Controller:
"""机器人控制算法基类"""
def __init__(self, model, data):
self.model = model
self.data = data
self.target_position = np.array([0.0, 0.0, 0.0])
self.target_orientation = np.array([1.0, 0.0, 0.0, 0.0])
self.gait_phase = 0.0
self.gait_speed = 1.0
self.last_update_time = time.time()
def set_target(self, position, orientation=None):
"""设置目标位置和方向"""
self.target_position = position
if orientation is not None:
self.target_orientation = orientation
def update_gait(self, dt):
"""更新步态相位"""
self.gait_phase = (self.gait_phase + dt * self.gait_speed) % (2 * math.pi)
return self.gait_phase
def compute_pd_control(self, joint_index, target_angle, kp=500, kd=50):
"""计算PD控制信号"""
current_angle = self.data.qpos[joint_index]
current_velocity = self.data.qvel[joint_index]
error = target_angle - current_angle
control = kp * error - kd * current_velocity
return control
def update(self, dt):
"""更新控制信号(子类实现)"""
raise NotImplementedError("Controller subclass must implement update method")
def reset(self):
"""重置控制器状态"""
self.gait_phase = 0.0
class WalkingController(Controller):
"""双足行走控制器"""
def __init__(self, model, data):
super().__init__(model, data)
# 步态参数
self.step_length = 0.3
self.step_height = 0.15
self.stance_width = 0.25
self.swing_percent = 0.4
self.hip_offset = 0.1
self.ankle_offset = 0.05
# 关节映射
self.joint_map = {}
try:
self.joint_map = {
"left_hip": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "left_hip"),
"left_knee": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "left_knee"),
"right_hip": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_hip"),
"right_knee": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_knee"),
"left_ankle": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "left_ankle"),
"right_ankle": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_ankle"),
}
except:
# 后备方案:使用索引代替名称
self.joint_map = {
"left_hip": 5,
"left_knee": 6,
"right_hip": 7,
"right_knee": 8,
"left_ankle": 9,
"right_ankle": 10
}
def inverse_kinematics(self, foot_pos, hip_pos, is_left=True):
"""计算腿部逆运动学"""
# 计算腿部向量
leg_vector = foot_pos - hip_pos
leg_length = np.linalg.norm(leg_vector)
# 计算髋关节角度
hip_pitch = math.atan2(leg_vector[2], leg_vector[0])
hip_roll = math.atan2(leg_vector[1], math.sqrt(leg_vector[0]**2 + leg_vector[2]**2))
# 计算膝关节角度
thigh_length = 0.4 # 大腿长度
shin_length = 0.4 # 小腿长度
knee_angle = math.acos((thigh_length**2 + shin_length**2 - leg_length**2) /
(2 * thigh_length * shin_length))
# 计算踝关节角度
ankle_pitch = hip_pitch
ankle_roll = hip_roll
# 根据左右腿调整符号
if is_left:
hip_roll = -hip_roll
ankle_roll = -ankle_roll
return hip_pitch, hip_roll, knee_angle, ankle_pitch, ankle_roll
def update(self, dt):
"""更新行走控制"""
gait_phase = self.update_gait(dt)
# 计算步态参数
swing_phase = gait_phase % (2 * math.pi)
left_in_swing = (swing_phase < math.pi * self.swing_percent) or \
(swing_phase > math.pi * (1 + self.swing_percent))
right_in_swing = not left_in_swing
# 获取机器人位置和方向
torso_pos = self.data.body("torso").xpos.copy()
torso_quat = self.data.body("torso").xquat.copy()
# 计算目标前进方向
target_direction = self.target_position - torso_pos
target_direction[2] = 0 # 忽略高度差
if np.linalg.norm(target_direction) > 0.1:
target_direction /= np.linalg.norm(target_direction)
else:
target_direction = np.array([1, 0, 0]) # 默认前进方向
# 计算髋关节位置
hip_height = 0.8
left_hip_pos = torso_pos + np.array([0, self.stance_width/2, -hip_height])
right_hip_pos = torso_pos + np.array([0, -self.stance_width/2, -hip_height])
# 计算脚部目标位置
step_progress = (gait_phase % (2 * math.pi)) / (2 * math.pi)
# 左脚轨迹
left_foot_target = left_hip_pos.copy()
left_foot_target[0] += self.step_length * math.sin(step_progress * math.pi)
left_foot_target[2] = -0.9 # 地面高度
if left_in_swing:
lift_height = self.step_height * math.sin(step_progress * math.pi / self.swing_percent)
left_foot_target[2] += lift_height
# 右脚轨迹
right_foot_target = right_hip_pos.copy()
right_foot_target[0] += self.step_length * math.sin((step_progress + 0.5) * math.pi)
right_foot_target[2] = -0.9 # 地面高度
if right_in_swing:
lift_height = self.step_height * math.sin((step_progress - 0.5) * math.pi / self.swing_percent)
right_foot_target[2] += lift_height
# 应用逆运动学计算关节角度
try:
left_hip_pitch, left_hip_roll, left_knee_angle, left_ankle_pitch, left_ankle_roll = \
self.inverse_kinematics(left_foot_target, left_hip_pos, is_left=True)
right_hip_pitch, right_hip_roll, right_knee_angle, right_ankle_pitch, right_ankle_roll = \
self.inverse_kinematics(right_foot_target, right_hip_pos, is_left=False)
# 应用PD控制
self.data.ctrl[self.joint_map["left_hip"]] = self.compute_pd_control(
self.joint_map["left_hip"], left_hip_pitch)
self.data.ctrl[self.joint_map["left_knee"]] = self.compute_pd_control(
self.joint_map["left_knee"], left_knee_angle)
self.data.ctrl[self.joint_map["left_ankle"]] = self.compute_pd_control(
self.joint_map["left_ankle"], left_ankle_pitch)
self.data.ctrl[self.joint_map["right_hip"]] = self.compute_pd_control(
self.joint_map["right_hip"], right_hip_pitch)
self.data.ctrl[self.joint_map["right_knee"]] = self.compute_pd_control(
self.joint_map["right_knee"], right_knee_angle)
self.data.ctrl[self.joint_map["right_ankle"]] = self.compute_pd_control(
self.joint_map["right_ankle"], right_ankle_pitch)
except KeyError:
# 后备控制方案
amplitude = 0.5
freq = 2.0
self.data.ctrl[5] = amplitude * math.sin(gait_phase) # left_hip
self.data.ctrl[6] = amplitude * math.sin(gait_phase + math.pi/2) # left_knee
self.data.ctrl[7] = amplitude * math.sin(gait_phase + math.pi) # right_hip
self.data.ctrl[8] = amplitude * math.sin(gait_phase + 3*math.pi/2) # right_knee
# ======================== 环境交互管理器 ========================
class EnvironmentManager:
"""管理仿真环境中的物体和交互"""
def __init__(self, model):
self.model = model
self.obstacles = []
self.targets = []
self.interactive_objects = []
self.create_environment()
def create_environment(self):
"""创建基本环境"""
# 添加一些障碍物
self.add_obstacle("box1", [3.0, 1.5, 0.5], [0.5, 0.5, 0.5])
self.add_obstacle("box2", [5.0, -1.0, 0.3], [0.8, 0.3, 0.3])
self.add_obstacle("cylinder1", [7.0, 0.0, 0.4], [0.4, 0.4, 0.4], geom_type="cylinder")
# 添加目标点
self.add_target("target1", [8.0, 0.0, 0.5])
self.add_target("target2", [10.0, 2.0, 0.5])
# 添加可交互物体
self.add_interactive_object("ball1", [2.0, -2.0, 0.5], 0.3)
def add_obstacle(self, name, position, size, geom_type="box", rgba=None):
"""添加障碍物"""
if rgba is None:
rgba = [0.8, 0.2, 0.2, 0.8] # 半透明红色
obstacle = {
"name": name,
"type": "obstacle",
"geom_type": geom_type,
"position": np.array(position),
"size": np.array(size),
"rgba": rgba
}
self.obstacles.append(obstacle)
def add_target(self, name, position, size=0.3, rgba=None):
"""添加目标点"""
if rgba is None:
rgba = [0.2, 0.8, 0.2, 1.0] # 绿色
target = {
"name": name,
"type": "target",
"position": np.array(position),
"size": size,
"rgba": rgba,
"reached": False
}
self.targets.append(target)
def add_interactive_object(self, name, position, size, mass=2.0, rgba=None):
"""添加可交互物体"""
if rgba is None:
rgba = [0.2, 0.2, 0.8, 1.0] # 蓝色
obj = {
"name": name,
"type": "interactive",
"position": np.array(position),
"size": size,
"mass": mass,
"rgba": rgba
}
self.interactive_objects.append(obj)
def check_collision(self, position, radius=0.3):
"""检查与障碍物的碰撞"""
for obstacle in self.obstacles:
if obstacle["geom_type"] == "box":
# 简化的AABB碰撞检测
min_corner = obstacle["position"] - obstacle["size"]
max_corner = obstacle["position"] + obstacle["size"]
closest_point = np.clip(position, min_corner, max_corner)
distance = np.linalg.norm(position - closest_point)
if distance < radius:
return True, obstacle["name"]
elif obstacle["geom_type"] == "cylinder":
# 圆柱体碰撞检测 (忽略高度)
dx = position[0] - obstacle["position"][0]
dy = position[1] - obstacle["position"][1]
distance = math.sqrt(dx*dx + dy*dy)
if distance < obstacle["size"][0] + radius:
return True, obstacle["name"]
return False, None
def check_target_reached(self, position, radius=0.5):
"""检查是否到达目标点"""
for target in self.targets:
if not target["reached"]:
distance = np.linalg.norm(position - target["position"])
if distance < radius:
target["reached"] = True
return True, target["name"]
return False, None
# ======================== 数据分析与可视化 ========================
class DataAnalyzer:
"""收集、分析和可视化仿真数据"""
def __init__(self):
self.data = {
"time": [],
"position": [],
"velocity": [],
"energy": [],
"stability": [],
"collisions": [],
"targets_reached": [],
"joint_angles": {},
"joint_torques": {}
}
self.start_time = time.time()
self.collision_count = 0
self.targets_reached = 0
def record_frame(self, data, robot):
"""记录当前帧的数据"""
current_time = time.time() - self.start_time
self.data["time"].append(current_time)
try:
# 记录位置和速度
torso_pos = robot.data.body("torso").xpos.copy()
torso_vel = robot.data.body("torso").cvel[3:6].copy() # 线性速度
self.data["position"].append(torso_pos)
self.data["velocity"].append(torso_vel)
# 计算并记录能量消耗
energy = 0
for i in range(robot.model.nu):
torque = robot.data.ctrl[i]
velocity = robot.data.qvel[i]
power = abs(torque * velocity)
energy += power * 0.01 # 假设时间步长为0.01
self.data["energy"].append(energy)
# 计算稳定性指标 (基于质心投影)
com = robot.data.subtree_com[0]
support_polygon = self.calculate_support_polygon(robot)
stability = self.calculate_stability(com, support_polygon)
self.data["stability"].append(stability)
# 记录碰撞和目标达成
self.data["collisions"].append(self.collision_count)
self.data["targets_reached"].append(self.targets_reached)
# 记录关节角度和扭矩
for i in range(robot.model.nu):
joint_name = f"joint_{i}"
if joint_name not in self.data["joint_angles"]:
self.data["joint_angles"][joint_name] = []
self.data["joint_torques"][joint_name] = []
self.data["joint_angles"][joint_name].append(robot.data.qpos[i])
self.data["joint_torques"][joint_name].append(robot.data.ctrl[i])
except Exception as e:
print(f"数据记录出错: {e}")
def calculate_support_polygon(self, robot):
"""计算支撑多边形"""
# 简化的支撑多边形计算 (基于脚部位置)
try:
left_foot_pos = robot.data.body("left_foot").xpos[:2]
right_foot_pos = robot.data.body("right_foot").xpos[:2]
return [left_foot_pos, right_foot_pos]
except:
# 后备方案:使用躯干位置
torso_pos = robot.data.body("torso").xpos[:2]
return [torso_pos - np.array([0.2, 0]), torso_pos + np.array([0.2, 0])]
def calculate_stability(self, com, support_polygon):
"""计算稳定性指标 (0-1)"""
try:
com_2d = com[:2]
min_dist = float('inf')
# 计算到支撑多边形边的最小距离
for i in range(len(support_polygon)):
p1 = support_polygon[i]
p2 = support_polygon[(i + 1) % len(support_polygon)]
# 计算点到线段的距离
line_vec = p2 - p1
point_vec = com_2d - p1
line_len = np.linalg.norm(line_vec)
line_unit = line_vec / line_len
proj_len = np.dot(point_vec, line_unit)
proj_len = max(0, min(line_len, proj_len))
proj_point = p1 + line_unit * proj_len
dist = np.linalg.norm(com_2d - proj_point)
if dist < min_dist:
min_dist = dist
# 稳定性指标 (距离越大,稳定性越低)
stability = max(0, 1.0 - min_dist * 2.0) # 假设0.5m距离为完全不稳定
return stability
except:
return 1.0 # 默认稳定
def report_collision(self):
"""记录碰撞事件"""
self.collision_count += 1
def report_target_reached(self):
"""记录目标达成事件"""
self.targets_reached += 1
def generate_report(self, filename="performance_report.pdf"):
"""生成性能报告"""
try:
fig, axs = plt.subplots(3, 2, figsize=(15, 15))
fig.suptitle("人形机器人性能分析报告", fontsize=16)
# 位置轨迹
positions = np.array(self.data["position"])
axs[0, 0].plot(positions[:, 0], positions[:, 1], 'b-')
axs[0, 0].set_title("运动轨迹")
axs[0, 0].set_xlabel("X (m)")
axs[0, 0].set_ylabel("Y (m)")
axs[0, 0].grid(True)
# 速度和能量
time = np.array(self.data["time"])
velocities = np.array([np.linalg.norm(v) for v in self.data["velocity"]])
axs[0, 1].plot(time, velocities, 'r-', label="速度")
axs[0, 1].set_title("速度变化")
axs[0, 1].set_xlabel("时间 (s)")
axs[0, 1].set_ylabel("速度 (m/s)")
axs[0, 1].grid(True)
ax2 = axs[0, 1].twinx()
energy = np.array(self.data["energy"])
cumulative_energy = np.cumsum(energy)
ax2.plot(time, cumulative_energy, 'g-', label="累计能耗")
ax2.set_ylabel("能耗 (J)")
# 稳定性和事件
stability = np.array(self.data["stability"])
axs[1, 0].plot(time, stability, 'm-')
axs[1, 0].set_title("稳定性指标")
axs[1, 0].set_xlabel("时间 (s)")
axs[1, 0].set_ylabel("稳定性 (0-1)")
axs[1, 0].grid(True)
collisions = np.array(self.data["collisions"])
targets = np.array(self.data["targets_reached"])
axs[1, 1].plot(time, collisions, 'r-', label="碰撞次数")
axs[1, 1].plot(time, targets, 'g-', label="达成目标数")
axs[1, 1].set_title("事件统计")
axs[1, 1].set_xlabel("时间 (s)")
axs[1, 1].legend()
axs[1, 1].grid(True)
# 关节角度和扭矩
if self.data["joint_angles"]:
joint_name = list(self.data["joint_angles"].keys())[0]
angles = np.array(self.data["joint_angles"][joint_name])
torques = np.array(self.data["joint_torques"][joint_name])
axs[2, 0].plot(time, angles, 'b-')
axs[2, 0].set_title(f"关节角度: {joint_name}")
axs[2, 0].set_xlabel("时间 (s)")
axs[2, 0].set_ylabel("角度 (rad)")
axs[2, 0].grid(True)
axs[2, 1].plot(time, torques, 'r-')
axs[2, 1].set_title(f"关节扭矩: {joint_name}")
axs[2, 1].set_xlabel("时间 (s)")
axs[2, 1].set_ylabel("扭矩 (Nm)")
axs[2, 1].grid(True)
plt.tight_layout()
plt.savefig(filename)
print(f"✅ 性能报告已保存到 {filename}")
return True
except Exception as e:
print(f"⚠️ 生成报告失败: {e}")
return False
# ======================== 增强版人形机器人仿真系统 ========================
class EnhancedHumanoidDemo:
"""人形机器人仿真演示增强版"""
def __init__(self):
"""初始化演示系统"""
print("🤖 人形机器人仿真演示系统 (增强版)")
print("=" * 60)
# 创建人形机器人模型
self.model = self._create_robot_model()
self.data = mujoco.MjData(self.model)
# 创建渲染器
self.renderer = MuJoCoRenderer(self.model)
self.renderer.data = self.data # 关联数据
# 创建控制器
self.controller = WalkingController(self.model, self.data)
# 创建环境管理器
self.environment = EnvironmentManager(self.model)
# 创建数据分析器
self.analyzer = DataAnalyzer()
# 初始化状态
self.current_target_index = 0
self.navigation_path = []
# 演示配置
self.demo_config = {
'duration': 60.0,
'enable_ai': True,
'enable_optimization': True,
'enable_adaptation': True,
'record_data': True,
'save_video': True,
'show_analysis': True
}
# 视频录制
self.video_writer = None
if self.demo_config['save_video']:
self.init_video_recording()
print("✅ 增强版演示系统初始化完成")
def _create_robot_model(self):
"""创建人形机器人模型(包含脚部传感器)"""
model_xml = """
<mujoco>
<option gravity="0 0 -9.81"/>
<asset>
<material name="grid" rgba="0.8 0.9 0.8 1"/>
<material name="body" rgba="0.5 0.7 0.9 1"/>
<material name="limb" rgba="0.6 0.8 0.4 1"/>
<material name="head" rgba="0.8 0.6 0.4 1"/>
<material name="foot" rgba="0.3 0.3 0.3 1"/>
</asset>
<worldbody>
<light name="light" pos="0 0 4" dir="0 0 -1"/>
<geom name="ground" type="plane" size="20 20 0.1" material="grid"/>
<!-- 机器人主体 -->
<body name="torso" pos="0 0 1.5">
<joint name="root_z" type="slide" axis="0 0 1" pos="0 0 0.5"/>
<joint name="root_x" type="slide" axis="1 0 0" pos="0 0 0.5"/>
<joint name="root_y" type="hinge" axis="0 1 0" pos="0 0 0.5"/>
<geom name="torso_geom" type="box" size="0.3 0.2 0.4" mass="10" material="body"/>
<!-- 头部 -->
<body name="head" pos="0 0 0.3">
<joint name="neck" type="ball" damping="0.01"/>
<geom name="head_geom" type="sphere" size="0.2" mass="3" material="head"/>
</body>
<!-- 手臂 -->
<body name="left_upper_arm" pos="0.3 0.2 0">
<joint name="left_shoulder" type="ball" damping="0.01"/>
<geom name="left_upper_arm_geom" type="capsule" fromto="0 0 0 0.2 0 0" size="0.05" mass="1.5" material="limb"/>
<body name="left_lower_arm" pos="0.2 0 0">
<joint name="left_elbow" type="hinge" axis="0 1 0" range="0 90"/>
<geom name="left_lower_arm_geom" type="capsule" fromto="0 0 0 0.2 0 0" size="0.04" mass="1.0" material="limb"/>
</body>
</body>
<body name="right_upper_arm" pos="0.3 -0.2 0">
<joint name="right_shoulder" type="ball" damping="0.01"/>
<geom name="right_upper_arm_geom" type="capsule" fromto="0 0 0 0.2 0 0" size="0.05" mass="1.5" material="limb"/>
<body name="right_lower_arm" pos="0.2 0 0">
<joint name="right_elbow" type="hinge" axis="0 1 0" range="0 90"/>
<geom name="right_lower_arm_geom" type="capsule" fromto="0 0 0 0.2 0 0" size="0.04" mass="1.0" material="limb"/>
</body>
</body>
<!-- 腿部 -->
<body name="left_upper_leg" pos="0 -0.1 -0.4">
<joint name="left_hip" type="ball" damping="0.01"/>
<geom name="left_upper_leg_geom" type="capsule" fromto="0 0 0 0 -0.1 -0.4" size="0.07" mass="4" material="limb"/>
<body name="left_lower_leg" pos="0 -0.1 -0.8">
<joint name="left_knee" type="hinge" axis="0 1 0" range="0 120"/>
<geom name="left_lower_leg_geom" type="capsule" fromto="0 0 0 0 0 -0.4" size="0.05" mass="3" material="limb"/>
<body name="left_foot" pos="0 0 -0.4">
<joint name="left_ankle" type="ball" damping="0.01"/>
<geom name="left_foot_geom" type="box" size="0.1 0.05 0.05" mass="1.5" material="foot"/>
<site name="left_foot_sensor" type="sphere" size="0.01" pos="0 0 -0.05" rgba="1 0 0 1"/>
</body>
</body>
</body>
<body name="right_upper_leg" pos="0 0.1 -0.4">
<joint name="right_hip" type="ball" damping="0.01"/>
<geom name="right_upper_leg_geom" type="capsule" fromto="0 0 0 0 0.1 -0.4" size="0.07" mass="4" material="limb"/>
<body name="right_lower_leg" pos="0 0.1 -0.8">
<joint name="right_knee" type="hinge" axis="0 1 0" range="0 120"/>
<geom name="right_lower_leg_geom" type="capsule" fromto="0 0 0 0 0 -0.4" size="0.05" mass="3" material="limb"/>
<body name="right_foot" pos="0 0 -0.4">
<joint name="right_ankle" type="ball" damping="0.01"/>
<geom name="right_foot_geom" type="box" size="0.1 0.05 0.05" mass="1.5" material="foot"/>
<site name="right_foot_sensor" type="sphere" size="0.01" pos="0 0 -0.05" rgba="1 0 0 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor joint="neck" gear="50"/>
<motor joint="left_shoulder" gear="100"/>
<motor joint="left_elbow" gear="80"/>
<motor joint="right_shoulder" gear="100"/>
<motor joint="right_elbow" gear="80"/>
<motor joint="left_hip" gear="150"/>
<motor joint="left_knee" gear="120"/>
<motor joint="right_hip" gear="150"/>
<motor joint="right_knee" gear="120"/>
<motor joint="left_ankle" gear="100"/>
<motor joint="right_ankle" gear="100"/>
</actuator>
<sensor>
<touch name="left_foot_contact" site="left_foot_sensor"/>
<touch name="right_foot_contact" site="right_foot_sensor"/>
</sensor>
</mujoco>
"""
try:
return mujoco.MjModel.from_xml_string(model_xml)
except Exception as e:
print(f"⚠️ 创建机器人模型失败: {e}")
# 创建简单后备模型
return mujoco.MjModel.from_xml_string("""
<mujoco>
<worldbody>
<light name="light" pos="0 0 4"/>
<geom name="ground" type="plane" size="10 10 0.1" rgba="0.8 0.9 0.8 1"/>
<body name="torso" pos="0 0 1">
<joint type="free"/>
<geom type="box" size="0.2 0.2 0.2" rgba="0.5 0.7 0.9 1"/>
<body name="left_leg" pos="0 -0.2 -0.3">
<joint type="hinge" axis="0 1 0"/>
<geom type="capsule" fromto="0 0 0 0 -0.2 -0.4" size="0.05" rgba="0.6 0.8 0.4 1"/>
</body>
<body name="right_leg" pos="0 0.2 -0.3">
<joint type="hinge" axis="0 1 0"/>
<geom type="capsule" fromto="0 0 0 0 0.2 -0.4" size="0.05" rgba="0.6 0.8 0.4 1"/>
</body>
</body>
</worldbody>
<actuator>
<motor joint="left_leg" gear="100"/>
<motor joint="right_leg" gear="100"/>
</actuator>
</mujoco>
""")
def init_video_recording(self):
"""初始化视频录制"""
try:
self.video_writer = imageio.get_writer(
f'simulation_{time.strftime("%Y%m%d_%H%M%S")}.mp4',
fps=30,
macro_block_size=None
)
print("🎥 视频录制已启动")
except Exception as e:
print(f"⚠️ 无法启动视频录制: {e}")
self.video_writer = None
def update_navigation(self):
"""更新导航目标"""
if not self.environment.targets:
return
# 获取当前目标
current_target = self.environment.targets[self.current_target_index]
# 如果目标已达成,移动到下一个目标
if current_target["reached"]:
self.current_target_index = (self.current_target_index + 1) % len(self.environment.targets)
current_target = self.environment.targets[self.current_target_index]
# 设置控制器目标
self.controller.set_target(current_target["position"])
def update_robot(self, dt):
"""更新机器人状态"""
if self.renderer.paused:
return
# 更新导航目标
self.update_navigation()
# 更新控制器
self.controller.update(dt)
# 应用物理模拟
mujoco.mj_step(self.model, self.data, nstep=1)
# 检测碰撞
torso_pos = self.data.body("torso").xpos
collision, obstacle_name = self.environment.check_collision(torso_pos)
if collision:
print(f"⚠️ 与障碍物 '{obstacle_name}' 发生碰撞!")
self.analyzer.report_collision()
# 检测目标达成
target_reached, target_name = self.environment.check_target_reached(torso_pos)
if target_reached:
print(f"🎯 达成目标 '{target_name}'!")
self.analyzer.report_target_reached()
def record_data(self, current_time):
"""记录演示数据"""
self.analyzer.record_frame(self.data, self)
def run_demo(self):
"""运行演示"""
print(f"🚀 开始演示,持续时间: {self.demo_config['duration']}秒")
print("=" * 60)
print("控制说明:")
print(" 方向键: 控制机器人移动")
print(" 空格键: 暂停/继续仿真")
print(" R 键: 重置仿真")
print(" C 键: 切换相机视角")
print(" ESC 键: 退出程序")
print("=" * 60)
start_time = time.time()
last_render_time = time.time()
last_data_record_time = time.time()
frame_count = 0
try:
while not glfw.window_should_close(self.renderer.window):
current_time = time.time() - start_time
# 更新机器人状态
dt = min(0.01, time.time() - last_render_time)
self.update_robot(dt)
# 记录数据 (每秒10次)
if time.time() - last_data_record_time > 0.1 and self.demo_config['record_data']:
self.record_data(current_time)
last_data_record_time = time.time()
# 渲染
self.renderer.render()
frame_count += 1
# 捕获视频帧
if self.video_writer:
frame = self.renderer.capture_frame()
self.video_writer.append_data(frame)
# 显示帧率
fps = frame_count / (current_time + 1e-5)
if frame_count % 30 == 0:
status = f"{self.renderer.title} - FPS: {fps:.1f}, Time: {current_time:.1f}/{self.demo_config['duration']:.1f}s"
status += f", Targets: {self.analyzer.targets_reached}/{len(self.environment.targets)}"
glfw.set_window_title(self.renderer.window, status)
# 检查演示是否结束
if current_time >= self.demo_config['duration']:
break
last_render_time = time.time()
print("\n✅ 演示完成!")
# 生成性能报告
if self.demo_config['show_analysis']:
self.analyzer.generate_report()
except Exception as e:
print(f"\n⛔ 演示出错: {e}")
import traceback
traceback.print_exc()
finally:
# 关闭视频录制
if self.video_writer:
self.video_writer.close()
print("🎥 视频录制已保存")
# 关闭渲染器
self.renderer.close()
def main():
"""主函数"""
# 创建演示实例
demo = EnhancedHumanoidDemo()
# 设置演示参数
demo.demo_config['duration'] = 60.0 # 60秒演示
demo.demo_config['save_video'] = True # 启用视频录制
demo.demo_config['show_analysis'] = True # 显示分析报告
# 运行演示
demo.run_demo()
if __name__ == "__main__":
# 设置环境变量(如果 MuJoCo 不在默认路径)
# os.environ['MUJOCO_PY_MUJOCO_PATH'] = 'path/to/mujoco'
main()
最新发布