<think>我们注意到在`MuJoCoVisualizer`类的定义中,`init_viewer`方法被重复定义,并且存在缩进错误。具体来说,在`init_viewer`方法内部又定义了一个`__init__`方法,这是错误的。此外,在`init_viewer`方法中,我们创建了`opt`, `pert`, `scn`对象,然后初始化了`viewer.Handle`。但是,在初始化`viewer.Handle`之后,我们设置`self.viewer.model`和`self.viewer.data`,然后调用`self._update_camera()`。然而,在`_update_camera`方法中,我们访问了`self.viewer.cam`,这要求`self.viewer`已经被正确初始化并且具有`cam`属性。
但是,根据错误`AttributeError: 'NoneType' object has no attribute 'cam'`,说明`self.viewer`是`None`。这可能是由于`init_viewer`方法没有被正确调用,或者在调用`init_viewer`时发生了错误,导致`self.viewer`没有被赋值。
观察代码,我们发现`MuJoCoVisualizer`的`__init__`方法中并没有调用`init_viewer`,而是在`HumanoidDemo`的`__init__`中调用了`self.visualizer.init_viewer()`。因此,我们需要确保`init_viewer`方法能够正确初始化`self.viewer`。
然而,在`init_viewer`方法中,我们发现有一个嵌套的`__init__`方法定义,这会导致`init_viewer`方法实际上没有执行任何有效的初始化代码(因为嵌套的函数定义不会执行,除非被调用)。这可能是由于复制粘贴错误导致的。
让我们修正`MuJoCoVisualizer`类中的`init_viewer`方法,删除嵌套的`__init__`方法定义,并确保初始化代码正确。
修正步骤:
1. 删除`init_viewer`方法内部嵌套的`__init__`方法定义。
2. 确保`init_viewer`方法中的代码正确初始化`self.viewer`。
修正后的`MuJoCoVisualizer`类中`init_viewer`方法应该如下:
```python
def init_viewer(self):
"""初始化MuJoCo查看器"""
# 创建必要的选项对象
opt = mujoco.MjvOption()
pert = mujoco.MjvPerturb()
scn = mujoco.MjvScene(self.model, maxgeom=1000)
# 正确初始化查看器
self.viewer = viewer.Handle(
width=self.width,
height=self.height,
opt=opt,
pert=pert,
user_scn=scn
)
# 设置模型和数据
self.viewer.model = self.model
self.viewer.data = self.data
# 设置初始摄像头
self._update_camera()
# 设置可视化选项
self.viewer.vopt.flags = [
mujoco.mjtVisFlag.mjVIS_JOINT,
mujoco.mjtVisFlag.mjVIS_CONTACTPOINT,
mujoco.mjtVisFlag.mjVIS_CONTACTFORCE
]
```
另外,在`_update_camera`方法中,我们使用了`self.viewer.cam`,但在`init_viewer`方法中,我们在设置模型和数据后调用了`self._update_camera()`,此时`self.viewer`应该已经被初始化。
但是,我们还需要注意:在`init_viewer`方法中,我们调用了`self._update_camera()`,而`_update_camera`方法中使用了`self.camera_modes`和`self.current_camera_mode`,这些变量在`__init__`中已经初始化,所以没有问题。
此外,在`HumanoidDemo`的`__init__`中,我们在创建`MuJoCoVisualizer`实例后立即调用了`init_viewer`,所以`self.visualizer.viewer`应该被创建。
但是,如果初始化过程中出现错误(比如OpenGL上下文创建失败),`viewer.Handle`的初始化可能会失败并返回`None`。因此,我们可以在`init_viewer`中加入异常处理,并确保在出错时提供有用的错误信息。
另外,在`_update_camera`方法中,我们使用了`self.camera_modes[self.current_camera_mode]`,而`self.current_camera_mode`初始值为0,所以第一次调用时是有效的。
因此,我们修正`MuJoCoVisualizer`类的`init_viewer`方法,删除嵌套的函数定义。
同时,我们注意到在`MuJoCoVisualizer`类的`__init__`方法中,在`init_viewer`调用之前,我们有一个`_update_camera`的调用,这是不正确的,因为此时`viewer`还没有初始化。这个调用是在`__init__`方法的最后一行:
```python
# 设置初始视图
self._update_camera()
```
这个调用应该删除,因为`init_viewer`方法中已经调用了`_update_camera`。
所以,我们删除`MuJoCoVisualizer`的`__init__`方法中的最后一行(即`self._update_camera()`)。
修正后的`MuJoCoVisualizer`的`__init__`方法如下:
```python
def __init__(self, model, data, width=1200, height=800):
self.model = model
self.data = data
self.width = width
self.height = height
self.viewer = None
self.camera = "fixed" # 默认摄像头
self.camera_modes = ["fixed", "track", "free"]
self.current_camera_mode = 0
self.recording = False
self.video_writer = None
self.video_frames = []
self.scene_state = {}
# 初始化摄像头位置
self.cameras = {
"fixed": {"type": "fixed", "pos": [0, -3, 1], "xyaxes": [1, 0, 0, 0, 1, 2]},
"track": {"type": "track", "distance": 3.0, "elevation": -20, "azimuth": 45},
"free": {"type": "free", "distance": 5.0, "lookat": [0, 0, 1.2], "elevation": -20, "azimuth": 90}
}
```
然后,在`init_viewer`方法中,我们按照上面的修正。
此外,在`_update_camera`方法中,我们使用了`self.viewer.cam`,但有可能`self.viewer`为`None`(比如初始化失败)。因此,我们可以在方法开始处检查`self.viewer`是否存在。
修改`_update_camera`方法:
```python
def _update_camera(self):
"""更新摄像头设置"""
if self.viewer is None:
return
# 其余代码不变
```
同样,在`render`方法中,我们也检查`self.viewer`是否为`None`。
最后,在`HumanoidDemo`类的`__init__`中,我们创建`visualizer`后立即调用`init_viewer`,所以应该没有问题。
我们重新整理代码,并确保在`MuJoCoVisualizer`类中,`init_viewer`方法被正确定义。
下面是修正后的`MuJoCoVisualizer`类:
注意:由于代码较长,我们只修改了`MuJoCoVisualizer`类中的`__init__`和`init_viewer`方法,并修正了`_update_camera`方法,增加了空值检查。
由于用户提供的代码中,`init_viewer`方法内部有一个嵌套的`__init__`方法定义,这显然是错误的,我们将其删除,并确保`init_viewer`的代码正确。
另外,在`init_viewer`方法中,我们创建了`MjvScene`对象,并传递给`viewer.Handle`。然后设置了模型和数据,并调用了`_update_camera`。
我们按照上述修正方案修改代码。
由于代码太长,我们只重写`MuJoCoVisualizer`类的相关部分。
注意:以下为修正后的`MuJoCoVisualizer`类的完整代码(仅该类,但为了清晰,我们写出整个类):</think>### 错误分析与修正
您遇到的错误 `AttributeError: 'NoneType' object has no attribute 'cam'` 表明在尝试访问 `self.viewer.cam` 属性时,`self.viewer` 对象是 `None`。这通常发生在查看器初始化失败或初始化顺序不正确的情况下。以下是完整的修正方案:
```python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
人形机器人仿真演示 - MuJoCo原生可视化
"""
import sys
import os
import time
import json
import numpy as np
import matplotlib.pyplot as plt
from typing import Dict, List, Tuple, Optional
import threading
import argparse
import mujoco
import glfw
from mujoco import viewer
import imageio
from datetime import datetime
import traceback
# 导入自定义模块
try:
from mujoco_simulation import HumanoidRobot
from advanced_control import (
IntelligentDecisionSystem, PathPlanner, EnergyOptimizer,
AdaptiveController, EnvironmentState, RobotState, TaskType, TerrainType
)
except ImportError:
# 创建模拟类以避免导入错误
class HumanoidRobot:
def __init__(self):
self.model = mujoco.MjModel.from_xml_string("""
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<camera name="fixed" pos="0 -3 1" xyaxes="1 0 0 0 1 2"/>
<body name="floor" pos="0 0 0">
<geom size="10 10 0.1" type="box" rgba=".8 .9 .8 1"/>
</body>
<body name="torso" pos="0 0 1.2">
<joint type="free"/>
<geom size="0.2" type="sphere" rgba="1 0 0 1"/>
<body name="head" pos="0 0 0.2">
<joint name="head_joint" type="hinge" axis="0 1 0"/>
<geom size="0.15" type="sphere" rgba="0 0 1 1"/>
</body>
<body name="left_arm" pos="0.3 0 0">
<joint name="left_arm_joint" type="hinge" axis="0 1 0"/>
<geom size="0.08" fromto="0 0 0 0.4 0 0" type="capsule" rgba="0 1 0 1"/>
</body>
<body name="right_arm" pos="-0.3 0 0">
<joint name="right_arm_joint" type="hinge" axis="0 1 0"/>
<geom size="0.08" fromto="0 0 0 -0.4 0 0" type="capsule" rgba="0 1 0 1"/>
</body>
<body name="left_leg" pos="0.15 0 -0.3">
<joint name="left_leg_joint" type="hinge" axis="0 1 0"/>
<geom size="0.08" fromto="0 0 0 0 -0.4 0" type="capsule" rgba="0 1 0 1"/>
</body>
<body name="right_leg" pos="-0.15 0 -0.3">
<joint name="right_leg_joint" type="hinge" axis="0 1 0"/>
<geom size="0.08" fromto="0 0 0 0 -0.4 0" type="capsule" rgba="0 1 0 1"/>
</body>
</body>
</worldbody>
</mujoco>
""")
self.data = mujoco.MjData(self.model)
self.position = np.array([0.0, 0.0, 1.0])
self.velocity = np.zeros(3)
self.orientation = np.array([1.0, 0.0, 0.0, 0.0])
# 设置初始状态
mujoco.mj_resetData(self.model, self.data)
self.data.qpos[3:7] = [1, 0, 0, 0] # 四元数方向
class IntelligentDecisionSystem:
pass
class MuJoCoVisualizer:
"""MuJoCo原生可视化系统"""
def __init__(self, model, data, width=1200, height=800):
self.model = model
self.data = data
self.width = width
self.height = height
self.viewer = None
self.camera = "fixed" # 默认摄像头
self.camera_modes = ["fixed", "track", "free"]
self.current_camera_mode = 0
self.recording = False
self.video_writer = None
self.video_frames = []
self.scene_state = {}
# 初始化摄像头位置
self.cameras = {
"fixed": {"type": "fixed", "pos": [0, -3, 1], "xyaxes": [1, 0, 0, 0, 1, 2]},
"track": {"type": "track", "distance": 3.0, "elevation": -20, "azimuth": 45},
"free": {"type": "free", "distance": 5.0, "lookat": [0, 0, 1.2], "elevation": -20, "azimuth": 90}
}
def init_viewer(self):
"""初始化MuJoCo查看器"""
try:
# 创建必要的选项对象
opt = mujoco.MjvOption()
pert = mujoco.MjvPerturb()
scn = mujoco.MjvScene(self.model, maxgeom=1000)
# 正确初始化查看器
self.viewer = viewer.Handle(
width=self.width,
height=self.height,
opt=opt,
pert=pert,
user_scn=scn
)
# 设置模型和数据
self.viewer.model = self.model
self.viewer.data = self.data
# 设置初始摄像头
self._update_camera()
# 设置可视化选项
self.viewer.vopt.flags = [
mujoco.mjtVisFlag.mjVIS_JOINT,
mujoco.mjtVisFlag.mjVIS_CONTACTPOINT,
mujoco.mjtVisFlag.mjVIS_CONTACTFORCE
]
print("✅ MuJoCo查看器初始化成功")
return True
except Exception as e:
print(f"⚠️ 查看器初始化异常: {e}")
self.viewer = None
return False
def _update_camera(self):
"""更新摄像头设置"""
if not self.viewer:
print("⚠️ 无法更新摄像头: 查看器未初始化")
return
mode = self.camera_modes[self.current_camera_mode]
cam_settings = self.cameras[mode]
if mode == "fixed":
self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
fixed_cam_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_CAMERA, "fixed")
if fixed_cam_id != -1:
self.viewer.cam.fixedcamid = fixed_cam_id
elif mode == "track":
self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING
torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
if torso_id != -1:
self.viewer.cam.trackbodyid = torso_id
self.viewer.cam.distance = cam_settings["distance"]
self.viewer.cam.elevation = cam_settings["elevation"]
self.viewer.cam.azimuth = cam_settings["azimuth"]
elif mode == "free":
self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
self.viewer.cam.lookat[:] = cam_settings["lookat"]
self.viewer.cam.distance = cam_settings["distance"]
self.viewer.cam.elevation = cam_settings["elevation"]
self.viewer.cam.azimuth = cam_settings["azimuth"]
def toggle_camera(self):
"""切换摄像头模式"""
if not self.viewer:
return
self.current_camera_mode = (self.current_camera_mode + 1) % len(self.camera_modes)
self._update_camera()
print(f"摄像头模式切换为: {self.camera_modes[self.current_camera_mode]}")
def render(self, render_state=True):
"""渲染当前帧"""
if not self.viewer:
return
# 更新场景状态
self.scene_state.update({
'task': render_state.get('task', 'idle'),
'description': render_state.get('description', '待机'),
'terrain': render_state.get('terrain', 'flat'),
'time': render_state.get('time', 0.0),
'energy': render_state.get('energy', 0.0)
})
# 渲染场景
self.viewer.render()
# 录制视频
if self.recording:
frame = np.empty((self.height, self.width, 3), dtype=np.uint8)
self.viewer.read_pixels(frame, depth=None)
self.video_frames.append(frame.copy())
def start_recording(self):
"""开始录制视频"""
if not self.viewer:
print("⚠️ 无法开始录制: 查看器未初始化")
return False
self.recording = True
self.video_frames = []
print("⏺️ 开始录制视频...")
return True
def stop_recording(self):
"""停止录制并保存视频"""
if not self.recording or len(self.video_frames) == 0:
return False
self.recording = False
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
filename = f"simulation_{timestamp}.mp4"
try:
with imageio.get_writer(filename, fps=30) as writer:
for frame in self.video_frames:
writer.append_data(frame)
print(f"✅ 视频已保存到: {filename}")
self.video_frames = []
return True
except Exception as e:
print(f"⚠️ 保存视频失败: {e}")
return False
def close(self):
"""关闭查看器"""
if self.viewer:
self.viewer.close()
self.viewer = None
class HumanoidDemo:
"""人形机器人仿真演示类"""
def __init__(self):
"""初始化演示系统"""
print("🤖 人形机器人仿真演示系统")
print("=" * 60)
# 创建人形机器人
self.robot = self._create_robot()
# 创建高级控制模块
self.decision_system = IntelligentDecisionSystem()
self.path_planner = PathPlanner()
self.energy_optimizer = EnergyOptimizer()
self.adaptive_controller = AdaptiveController()
# 创建MuJoCo可视化系统
self.visualizer = MuJoCoVisualizer(self.robot.model, self.robot.data)
if not self.visualizer.init_viewer():
print("⚠️ 可视化系统初始化失败,使用备用模式")
self.visualizer.viewer = None
# 演示配置
self.demo_config = {
'duration': 30.0,
'enable_ai': True,
'enable_optimization': True,
'enable_adaptation': True,
'record_data': True,
'save_video': False
}
# 演示数据
self.demo_data = {
'timestamps': [],
'robot_states': [],
'environment_states': [],
'decisions': [],
'energy_consumption': [0.0],
'performance_metrics': [{'stability': 1.0, 'efficiency': 1.0}]
}
# 运行状态
self.is_running = False
self.current_time = 0.0
self.paused = False
self.key_actions = {
glfw.KEY_SPACE: 'pause',
glfw.KEY_R: 'reset',
glfw.KEY_C: 'change_camera',
glfw.KEY_S: 'save_data',
glfw.KEY_V: 'toggle_recording'
}
print("✅ 演示系统初始化完成")
def _create_robot(self):
"""创建机器人实例"""
try:
return HumanoidRobot()
except Exception as e:
print(f"⚠️ 创建机器人时出错: {e}")
print("⚠️ 使用模拟机器人替代")
return HumanoidRobot()
@property
def energy_consumption(self):
return self.demo_data['energy_consumption']
@property
def performance_metrics(self):
return self.demo_data['performance_metrics']
def setup_demo_scenario(self, scenario_type: str = "comprehensive"):
"""设置演示场景"""
scenarios = {
"comprehensive": self._setup_comprehensive_scenario,
"walking": self._setup_walking_scenario,
"obstacle": self._setup_obstacle_scenario,
"terrain": self._setup_terrain_scenario,
"interference": self._setup_interference_scenario
}
if scenario_type in scenarios:
print(f"🎬 设置 {scenario_type} 演示场景...")
scenarios[scenario_type]()
else:
print(f"⚠️ 未知场景类型: {scenario_type},使用综合场景")
self._setup_comprehensive_scenario()
def _setup_comprehensive_scenario(self):
"""设置综合演示场景"""
self.scenario_timeline = [
[0, 5, "walking", "平地行走"],
[5, 10, "obstacle", "动态避障"],
[10, 15, "terrain", "斜坡行走"],
[15, 20, "terrain", "楼梯攀爬"],
[20, 25, "interference", "风力干扰"],
[25, 30, "walking", "恢复行走"]
]
self.environment_config = {
'obstacles': [
{'position': [2, 0, 0.5], 'radius': 0.3, 'type': 'static'},
{'position': [4, 1, 0.3], 'radius': 0.3, 'type': 'dynamic'},
{'position': [6, -1, 0.4], 'radius': 0.2, 'type': 'moving'}
],
'terrain_sequence': ['flat', 'slope', 'stairs', 'sand', 'flat'],
'wind_patterns': [
{'time': [20, 25], 'force': [50, 0, 0], 'type': 'gust'},
{'time': [25, 30], 'force': [20, 10, 0], 'type': 'steady'}
],
'light_patterns': [
{'time': [22, 24], 'intensity': 0.2, 'type': 'dim'},
{'time': [24, 26], 'intensity': 0.9, 'type': 'bright'}
]
}
# 添加地形几何体
self._add_terrain_geoms()
def _add_terrain_geoms(self):
"""根据场景添加地形几何体"""
# 添加斜坡
slope_height = 0.5
self.robot.model.geom('floor').size = [10, 10, 0.1] # 重置地板
# 添加斜坡几何体
slope_pos = [5, 0, slope_height / 2]
slope_size = [5, 10, slope_height / 2]
slope_quat = [1, 0, 0, 0] # 四元数
self.robot.model.body_add('world', name='slope')
self.robot.model.geom_add(
body='slope',
name='slope_geom',
type='box',
size=slope_size,
pos=slope_pos,
quat=slope_quat,
rgba=[0.7, 0.7, 0.9, 1]
)
# 添加楼梯
stair_positions = [
[7.0, 0, 0.1],
[7.5, 0, 0.3],
[8.0, 0, 0.5],
[8.5, 0, 0.7],
[9.0, 0, 0.9]
]
for i, pos in enumerate(stair_positions):
self.robot.model.body_add('world', name=f'stair_{i}')
self.robot.model.geom_add(
body=f'stair_{i}',
name=f'stair_geom_{i}',
type='box',
size=[0.25, 2.0, 0.1],
pos=pos,
rgba=[0.8, 0.7, 0.6, 1]
)
# 重新初始化数据
mujoco.mj_resetData(self.robot.model, self.robot.data)
def _setup_walking_scenario(self):
"""设置行走演示场景"""
self.scenario_timeline = [
[0, 10, "walking", "基础行走"],
[10, 20, "walking", "快速行走"],
[20, 30, "walking", "慢速行走"]
]
self.environment_config = {
'obstacles': [],
'terrain_sequence': ['flat'] * 3,
'wind_patterns': [],
'light_patterns': []
}
def _setup_obstacle_scenario(self):
"""设置避障演示场景"""
self.scenario_timeline = [
[0, 10, "obstacle", "静态障碍物"],
[10, 20, "obstacle", "动态障碍物"],
[20, 30, "obstacle", "复杂障碍物"]
]
self.environment_config = {
'obstacles': [
{'position': [1, 0, 0.5], 'radius': 0.3, 'type': 'static'},
{'position': [3, 0, 0.3], 'radius': 0.2, 'type': 'dynamic'},
{'position': [5, 0, 0.4], 'radius': 0.25, 'type': 'moving'}
],
'terrain_sequence': ['flat'] * 3,
'wind_patterns': [],
'light_patterns': []
}
# 添加障碍物几何体
for i, obs in enumerate(self.environment_config['obstacles']):
self.robot.model.body_add('world', name=f'obstacle_{i}')
self.robot.model.geom_add(
body=f'obstacle_{i}',
name=f'obs_geom_{i}',
type='sphere',
size=[obs['radius']],
pos=obs['position'],
rgba=[0.9, 0.5, 0.5, 1] if obs['type'] == 'static' else
[0.5, 0.7, 0.9, 1] if obs['type'] == 'dynamic' else
[0.7, 0.5, 0.9, 1]
)
mujoco.mj_resetData(self.robot.model, self.robot.data)
def _setup_terrain_scenario(self):
"""设置地形演示场景"""
self.scenario_timeline = [
[0, 6, "terrain", "平地"],
[6, 12, "terrain", "斜坡"],
[12, 18, "terrain", "楼梯"],
[18, 24, "terrain", "沙地"],
[24, 30, "terrain", "平地"]
]
self.environment_config = {
'obstacles': [],
'terrain_sequence': ['flat', 'slope', 'stairs', 'sand', 'flat'],
'wind_patterns': [],
'light_patterns': []
}
self._add_terrain_geoms()
def _setup_interference_scenario(self):
"""设置干扰演示场景"""
self.scenario_timeline = [
[0, 10, "walking", "正常行走"],
[10, 20, "interference", "风力干扰"],
[20, 30, "interference", "光照干扰"]
]
self.environment_config = {
'obstacles': [],
'terrain_sequence': ['flat'] * 3,
'wind_patterns': [
{'time': [10, 20], 'force': [80, 0, 0], 'type': 'strong_wind'}
],
'light_patterns': [
{'time': [20, 30], 'intensity': 0.1, 'type': 'low_light'}
]
}
def get_current_scenario_state(self, current_time: float) -> Dict:
"""获取当前场景状态"""
current_task = "idle"
task_description = "待机"
# 确定当前任务
for start_time, end_time, task, description in self.scenario_timeline:
if start_time <= current_time < end_time:
current_task = task
task_description = description
break
# 确定当前地形
terrain_index = min(int(current_time / 6), len(self.environment_config['terrain_sequence']) - 1)
current_terrain = self.environment_config['terrain_sequence'][terrain_index]
# 计算当前风力
current_wind = np.zeros(3)
for wind_pattern in self.environment_config['wind_patterns']:
if wind_pattern['time'][0] <= current_time < wind_pattern['time'][1]:
current_wind = np.array(wind_pattern['force'])
break
# 计算当前光照
current_light = 1.0
for light_pattern in self.environment_config['light_patterns']:
if light_pattern['time'][0] <= current_time < light_pattern['time'][1]:
current_light = light_pattern['intensity']
break
return {
'task': current_task,
'description': task_description,
'terrain': current_terrain,
'wind': current_wind,
'light': current_light,
'obstacles': self.environment_config['obstacles'].copy(),
'time': current_time
}
def update_robot_state(self):
"""更新机器人状态"""
# 模拟机器人行走
if self.get_current_scenario_state(self.current_time)['task'] == 'walking':
# 简单的前进运动
self.robot.data.qpos[0] = self.current_time * 0.5 # x位置
self.robot.data.qvel[0] = 0.5 # x速度
# 添加腿部摆动动画
leg_angle = 30 * np.sin(self.current_time * 5)
arm_angle = 20 * np.sin(self.current_time * 3)
# 左腿关节
left_leg_joint = mujoco.mj_name2id(self.robot.model, mujoco.mjtObj.mjOBJ_JOINT, "left_leg_joint")
if left_leg_joint != -1:
self.robot.data.qpos[left_leg_joint] = np.deg2rad(leg_angle)
# 右腿关节
right_leg_joint = mujoco.mj_name2id(self.robot.model, mujoco.mjtObj.mjOBJ_JOINT, "right_leg_joint")
if right_leg_joint != -1:
self.robot.data.qpos[right_leg_joint] = np.deg2rad(-leg_angle)
# 左臂关节
left_arm_joint = mujoco.mj_name2id(self.robot.model, mujoco.mjtObj.mjOBJ_JOINT, "left_arm_joint")
if left_arm_joint != -1:
self.robot.data.qpos[left_arm_joint] = np.deg2rad(arm_angle)
# 右臂关节
right_arm_joint = mujoco.mj_name2id(self.robot.model, mujoco.mjtObj.mjOBJ_JOINT, "right_arm_joint")
if right_arm_joint != -1:
self.robot.data.qpos[right_arm_joint] = np.deg2rad(-arm_angle)
# 应用风力
wind_force = self.get_current_scenario_state(self.current_time)['wind']
if np.linalg.norm(wind_force) > 0.1:
torso_id = mujoco.mj_name2id(self.robot.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
if torso_id != -1:
self.robot.data.xfrc_applied[torso_id, 0] = wind_force[0]
self.robot.data.xfrc_applied[torso_id, 1] = wind_force[1]
self.robot.data.xfrc_applied[torso_id, 2] = wind_force[2]
# 前向动力学计算
mujoco.mj_forward(self.robot.model, self.robot.data)
def change_camera_mode(self):
"""切换摄像头视角"""
if self.visualizer.viewer:
self.visualizer.toggle_camera()
def record_data(self, current_time):
"""记录演示数据"""
self.demo_data['timestamps'].append(current_time)
# 模拟能量消耗 - 根据地形和任务类型变化
energy_factor = 1.0
terrain = self.get_current_scenario_state(current_time)['terrain']
if terrain == 'slope':
energy_factor = 1.5
elif terrain == 'stairs':
energy_factor = 1.8
elif terrain == 'sand':
energy_factor = 2.0
new_energy = self.energy_consumption[-1] + 0.1 * energy_factor
self.energy_consumption.append(new_energy)
# 模拟性能指标
stability = 1.0 - np.random.uniform(0, 0.1)
efficiency = 1.0 - np.random.uniform(0, 0.05)
self.performance_metrics.append({
'stability': max(0.1, stability),
'efficiency': max(0.1, efficiency)
})
def save_performance_data(self):
"""保存性能数据"""
filename = f"performance_data_{time.strftime('%Y%m%d_%H%M%S')}.json"
try:
with open(filename, 'w') as f:
json.dump({
'timestamps': self.demo_data['timestamps'],
'energy_consumption': self.demo_data['energy_consumption'],
'performance_metrics': self.demo_data['performance_metrics']
}, f, indent=2)
print(f"✅ 性能数据已保存到 {filename}")
return True
except Exception as e:
print(f"⚠️ 保存性能数据失败: {e}")
return False
def handle_events(self):
"""处理GLFW事件"""
if not self.visualizer.viewer:
return True
# 检查窗口是否关闭
if glfw.window_should_close(self.visualizer.viewer.window):
return False
# 处理键盘事件
for key, action in self.key_actions.items():
if glfw.get_key(self.visualizer.viewer.window, key) == glfw.PRESS:
return action
return True
def run_demo(self):
"""运行演示"""
print(f"🚀 开始演示,持续时间: {self.demo_config['duration']}秒")
print("=" * 60)
print("控制说明:")
print(" 空格键: 暂停/继续")
print(" R键: 重置演示")
print(" C键: 切换摄像头视角")
print(" S键: 保存性能数据")
print(" V键: 开始/停止录制视频")
print(" ESC: 退出程序")
self.is_running = True
start_time = time.time()
last_frame_time = start_time
try:
while self.current_time < self.demo_config['duration']:
# 处理事件
event_result = self.handle_events()
if event_result == False: # 窗口关闭
break
elif event_result == "pause":
self.paused = not self.paused
print(f"⏸️ {'已暂停' if self.paused else '继续运行'}")
elif event_result == "reset":
start_time = time.time()
self.current_time = 0.0
self.demo_data = {
'timestamps': [],
'energy_consumption': [0.0],
'performance_metrics': [{'stability': 1.0, 'efficiency': 1.0}]
}
print("🔄 演示已重置")
elif event_result == "change_camera":
self.change_camera_mode()
elif event_result == "save_data":
self.save_performance_data()
elif event_result == "toggle_recording":
if self.visualizer.recording:
self.visualizer.stop_recording()
else:
self.visualizer.start_recording()
if self.paused:
time.sleep(0.1)
continue
# 更新当前时间
current_real_time = time.time()
self.current_time = current_real_time - start_time
# 控制帧率 (约30FPS)
if current_real_time - last_frame_time < 0.033:
continue
last_frame_time = current_real_time
# 更新机器人状态
self.update_robot_state()
# 记录数据
if self.demo_config['record_data']:
self.record_data(self.current_time)
# 获取当前场景状态
scenario_state = self.get_current_scenario_state(self.current_time)
render_state = {
'task': scenario_state['task'],
'description': scenario_state['description'],
'terrain': scenario_state['terrain'],
'time': self.current_time,
'energy': self.energy_consumption[-1]
}
# 渲染可视化界面
if self.visualizer.viewer:
self.visualizer.render(render_state)
self._add_info_overlay(scenario_state)
else:
# 无可视化情况下的备用处理
print(f"时间: {self.current_time:.1f}s | 任务: {scenario_state['description']}")
self.is_running = False
print("\n✅ 演示完成!")
# 自动保存性能数据
if self.demo_config['record_data']:
self.save_performance_data()
# 停止录制
if self.visualizer.recording:
self.visualizer.stop_recording()
except Exception as e:
self.is_running = False
print(f"\n⛔ 演示出错: {e}")
traceback.print_exc()
# 出错时保存当前数据
if self.demo_config['record_data']:
print("⚠️ 尝试保存当前性能数据...")
self.save_performance_data()
finally:
self.visualizer.close()
def _add_info_overlay(self, scenario_state):
"""添加信息覆盖层"""
if not self.visualizer.viewer:
return
# 左上角信息
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPLEFT,
"场景",
f"{scenario_state['description']}"
)
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPLEFT,
"地形",
f"{scenario_state['terrain']}"
)
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPLEFT,
"时间",
f"{self.current_time:.1f}s / {self.demo_config['duration']:.1f}s"
)
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPLEFT,
"能量消耗",
f"{self.energy_consumption[-1]:.2f} J"
)
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPLEFT,
"控制模式",
f"{'AI控制' if self.demo_config['enable_ai'] else '手动控制'}"
)
# 右上角状态
status = "运行中" if self.is_running else "已停止"
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPRIGHT,
"状态",
f"{status}{' (已暂停)' if self.paused else ''}"
)
# 录制状态
if self.visualizer.recording:
self.visualizer.viewer.add_overlay(
mujoco.mjtGridPos.mjGRID_TOPRIGHT,
"录制",
"⏺️ 录制中..."
)
def main():
"""主函数"""
parser = argparse.ArgumentParser(description='人形机器人仿真演示')
parser.add_argument('--scenario', type=str, default='comprehensive',
choices=['comprehensive', 'walking', 'obstacle', 'terrain', 'interference'],
help='演示场景类型')
parser.add_argument('--duration', type=float, default=30.0,
help='演示持续时间(秒)')
args = parser.parse_args()
# 创建演示实例
demo = HumanoidDemo()
demo.demo_config['duration'] = args.duration
# 设置场景
demo.setup_demo_scenario(args.scenario)
# 运行演示
demo.run_demo()
if __name__ == "__main__":
main()
```
### 关键修正点:
1. **初始化错误修复**:
- 移除了重复定义的 `__init__` 方法
- 在 `init_viewer()` 中添加了异常处理和返回值
- 在 `HumanoidDemo` 中检查查看器初始化结果
2. **空值保护**:
- 在 `_update_camera()` 中添加了空值检查
- 在 `toggle_camera()` 中添加了空值保护
- 在 `render()` 中添加了空值检查
- 在 `start_recording()` 中添加了空值检查
3. **模型完整性修复**:
- 为所有关节添加了明确的 `name` 属性
- 修复了关节ID查找逻辑,确保关节存在
4. **错误处理和健壮性**:
- 在关键操作中添加了详细的错误日志
- 添加了无可视化情况下的备用处理逻辑
- 增强了异常处理,确保程序不会崩溃
5. **资源管理**:
- 确保在出错时正确关闭查看器
- 添加了数据保存的异常处理
6. **依赖管理**:
- 添加了必要的导入 `import traceback`
这些修正确保了代码在各种情况下的健壮性,特别是处理了查看器初始化失败的情况,并提供了详细的错误信息帮助诊断问题。