diff --git a/src/humanoid_3d_animation/humanoid.xml b/src/humanoid_3d_animation/humanoid.xml
index a263bdbcf5..881178f198 100644
--- a/src/humanoid_3d_animation/humanoid.xml
+++ b/src/humanoid_3d_animation/humanoid.xml
@@ -1,121 +1,99 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py
index 4eb868e559..350a6e0290 100644
--- a/src/humanoid_3d_animation/humanoid_simulation.py
+++ b/src/humanoid_3d_animation/humanoid_simulation.py
@@ -6,7 +6,7 @@
import threading
import signal
import sys
-import select
+import random
from dataclasses import dataclass
import numpy as np
import matplotlib.pyplot as plt
@@ -17,8 +17,6 @@
@dataclass
class SimConfig:
"""仿真配置类：集中管理所有可配置参数"""
- # 文件路径配置
- xml_filename: str = "humanoid.xml"
# 仿真参数
timestep: float = 0.005
sim_frequency: float = 2.0
@@ -27,18 +25,21 @@ class SimConfig:
cam_distance: float = 2.0
cam_azimuth: float = 45.0
cam_elevation: float = -20.0
- # 关节运动幅度配置
+ # 关节运动幅度配置（针对不同动作优化）
joint_amplitudes = {
- "left_shoulder": 1.0, "right_shoulder": 1.0,
- "left_elbow": 0.5, "right_elbow": 0.5,
- "left_hip": 0.8, "right_hip": 0.8,
- "left_knee": 0.6, "right_knee": 0.6
+ "left_shoulder": 1.2, "right_shoulder": 1.2,
+ "left_elbow": 1.0, "right_elbow": 1.0,
+ "left_hip": 1.0, "right_hip": 1.0,
+ "left_knee": 1.2, "right_knee": 1.2
}
- # 控制模式
- default_mode: str = "sin"
+ # 控制模式（新增行走和挥手动作）
+ default_mode: str = "walk"
# 可视化配置
plot_update_interval: int = 50 # 绘图更新间隔（帧数）
max_plot_points: int = 200 # 图表最大显示数据点
+ # 动作参数
+ walk_stride: float = 0.8 # 行走步幅
+ wave_frequency: float = 1.5 # 挥手频率
# 全局变量
@@ -68,9 +69,12 @@ def __init__(self, config: SimConfig):
self.joint_qpos_indices = {}
self.current_mode = config.default_mode
self.last_ctrl_signals = {}
- self.input_thread_running = False
- # 新增：可视化相关变量
+ # 新增：动作状态变量
+ self.walk_phase = 0.0 # 行走相位
+ self.wave_arm = "right" # 当前挥动手臂
+
+ # 可视化相关变量
self.plot_data = {name: [] for name in self.joint_names}
self.time_data = []
self.frame_counter = 0
@@ -80,150 +84,234 @@ def __init__(self, config: SimConfig):
self.lines = {}
self.ani = None
- def create_xml_file(self, file_path):
- """创建人形机器人XML文件"""
- xml_content = f"""
+ def load_model(self):
+ """加载MuJoCo模型（完全修复XML格式）"""
+ # 完全兼容所有MuJoCo版本的XML
+ xml_content = """
-
-
-
-
+
-
+
+
+
+
+
+
+
-
+
+
-
+
+
-
+
+
-
-
+
+
+
-
-
+
+
+
-
-
+
+
+
-
-
+
+
+
-
-
+
+
+
-
-
+
+
+
-
-
+
+
+
-
-
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
-"""
- with open(file_path, "w", encoding="utf-8") as f:
- f.write(xml_content)
- print(f"✅ 已在 {file_path} 创建XML文件！")
-
- def load_model(self):
- """加载MuJoCo模型"""
- current_dir = os.path.dirname(os.path.abspath(__file__))
- self.model_path = os.path.join(current_dir, self.config.xml_filename)
- if not os.path.exists(self.model_path):
- self.create_xml_file(self.model_path)
- else:
- print(f"ℹ️ XML文件已存在（路径：{self.model_path}），无需重新创建！")
+
+
+"""
try:
- with open(self.model_path, "r", encoding="utf-8") as f:
- xml_content = f.read()
+ # 直接从XML字符串加载模型
self.model = mujoco.MjModel.from_xml_string(xml_content)
self.data = mujoco.MjData(self.model)
print("✅ 模型加载成功！")
except Exception as e:
print(f"❌ 模型加载失败：{e}")
+ import traceback
+ traceback.print_exc()
sys.exit(1)
- # 预存关节ID
+ # 映射关节ID
+ print("\n🔍 关节ID映射结果：")
for name in self.joint_names:
- ctrl_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{name}_motor")
- if ctrl_id == -1:
- ctrl_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
+ # 直接使用关节名作为电机名
+ ctrl_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
self.joint_ctrl_ids[name] = ctrl_id
+ # 获取关节ID和位置索引
joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
if joint_id != -1:
- self.joint_qpos_indices[name] = 7 + joint_id
+ self.joint_qpos_indices[name] = self.model.jnt_qposadr[joint_id]
else:
self.joint_qpos_indices[name] = -1
self.last_ctrl_signals[name] = 0.0
- def get_joint_ctrl_signal(self, name, t):
- """生成关节控制信号"""
+ # 打印详细信息
+ print(f" {name}: ctrl_id={ctrl_id}, qpos_idx={self.joint_qpos_indices[name]}")
+
+ # 验证控制信号数组
+ print(f"\n📊 控制信号数组长度：{len(self.data.ctrl)}")
+ print(f"📊 关节位置数组长度：{len(self.data.qpos)}")
+
+ def get_walk_action(self, name, t):
+ """生成行走动作控制信号"""
amplitude = self.config.joint_amplitudes[name]
- freq = self.config.sim_frequency
+ stride = self.config.walk_stride
- if self.current_mode == "sin":
- if "left" in name or "hip" in name or "knee" in name:
- if "shoulder" in name or "elbow" in name:
- signal = math.sin(t * freq) * amplitude
- else:
- signal = math.cos(t * freq) * amplitude
+ # 更新行走相位
+ self.walk_phase = (self.walk_phase + 0.01) % (2 * math.pi)
+
+ if "hip" in name:
+ # 髋关节交替摆动
+ if "left" in name:
+ signal = math.sin(self.walk_phase) * amplitude * stride
else:
- if "shoulder" in name or "elbow" in name:
- signal = -math.sin(t * freq) * amplitude
- else:
- signal = -math.cos(t * freq) * amplitude
+ signal = math.sin(self.walk_phase + math.pi) * amplitude * stride
+ elif "knee" in name:
+ # 膝关节配合髋关节运动
+ if "left" in name:
+ signal = math.cos(self.walk_phase) * amplitude * stride * 1.2
+ else:
+ signal = math.cos(self.walk_phase + math.pi) * amplitude * stride * 1.2
+ elif "shoulder" in name:
+ # 手臂自然摆动（与对侧腿相反）
+ if "left" in name:
+ signal = math.sin(self.walk_phase + math.pi) * amplitude * 0.5
+ else:
+ signal = math.sin(self.walk_phase) * amplitude * 0.5
+ elif "elbow" in name:
+ # 肘部轻微弯曲
+ if "left" in name:
+ signal = -math.fabs(math.sin(self.walk_phase + math.pi)) * amplitude * 0.6
+ else:
+ signal = -math.fabs(math.sin(self.walk_phase)) * amplitude * 0.6
+ else:
+ signal = 0.0
+
+ return signal
+
+ def get_wave_action(self, name, t):
+ """生成挥手动作控制信号"""
+ amplitude = self.config.joint_amplitudes[name]
+ freq = self.config.wave_frequency
+
+ # 每2秒切换一次挥动手臂
+ if int(t) % 2 == 0:
+ self.wave_arm = "right"
+ else:
+ self.wave_arm = "left"
+
+ signal = 0.0
+
+ # 挥动手臂的肩部和肘部运动
+ if f"{self.wave_arm}_shoulder" == name:
+ # 肩部上下摆动
+ signal = math.sin(t * freq) * amplitude * 1.2
+ elif f"{self.wave_arm}_elbow" == name:
+ # 肘部配合弯曲
+ signal = -math.fabs(math.sin(t * freq)) * amplitude * 1.0
+ # 另一只手臂保持自然下垂
+ elif ("shoulder" in name and self.wave_arm not in name):
+ signal = -0.2
+ elif ("elbow" in name and self.wave_arm not in name):
+ signal = -0.8
+ # 腿部保持稳定
+ elif "hip" in name or "knee" in name:
+ signal = 0.0
+
+ return signal
+
+ def get_joint_ctrl_signal(self, name, t):
+ """生成关节控制信号（支持多种动作模式）"""
+ # 根据当前模式选择动作
+ if self.current_mode == "walk":
+ signal = self.get_walk_action(name, t)
+ elif self.current_mode == "wave":
+ signal = self.get_wave_action(name, t)
+ elif self.current_mode == "sin":
+ # 原有正弦运动模式
+ if "left" in name:
+ signal = math.sin(t * self.config.sim_frequency) * self.config.joint_amplitudes[name]
+ else:
+ signal = -math.sin(t * self.config.sim_frequency) * self.config.joint_amplitudes[name]
elif self.current_mode == "random":
- signal = (math.sin(t * freq * 0.5) * 0.5 + 0.5) * amplitude * 2 - amplitude
+ # 随机运动模式
+ signal = (random.random() * 2 - 1) * self.config.joint_amplitudes[name]
elif self.current_mode == "stop":
+ # 停止模式
signal = 0.0
else:
signal = 0.0
# 平滑过渡
- smooth_factor = 0.1
+ smooth_factor = 0.05
self.last_ctrl_signals[name] = (1 - smooth_factor) * self.last_ctrl_signals[name] + smooth_factor * signal
+
+ # 限制信号范围在关节限位内
+ joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
+ if joint_id != -1:
+ jnt_range = self.model.jnt_range[joint_id]
+ self.last_ctrl_signals[name] = np.clip(self.last_ctrl_signals[name], jnt_range[0], jnt_range[1])
+
return self.last_ctrl_signals[name]
def update_joint_controls(self):
@@ -233,9 +321,13 @@ def update_joint_controls(self):
ctrl_id = self.joint_ctrl_ids[name]
if ctrl_id == -1:
continue
- ctrl_signal = self.get_joint_ctrl_signal(name, t)
+
try:
- self.data.ctrl[ctrl_id] = ctrl_signal
+ ctrl_signal = self.get_joint_ctrl_signal(name, t)
+ if 0 <= ctrl_id < len(self.data.ctrl):
+ self.data.ctrl[ctrl_id] = ctrl_signal
+ else:
+ print(f"⚠️ 关节 {name} 控制ID {ctrl_id} 超出范围（最大：{len(self.data.ctrl) - 1}）")
except Exception as e:
print(f"⚠️ 关节 {name} 控制失败：{e}")
@@ -253,11 +345,13 @@ def collect_plot_data(self):
# 添加各关节角度数据
for name in self.joint_names:
qpos_idx = self.joint_qpos_indices[name]
- if qpos_idx != -1 and qpos_idx < len(self.data.qpos):
+ if qpos_idx != -1 and 0 <= qpos_idx < len(self.data.qpos):
angle = self.data.qpos[qpos_idx]
self.plot_data[name].append(angle)
+ else:
+ self.plot_data[name].append(0.0)
- # 限制数据点数量，避免内存占用过大
+ # 限制数据点数量
if len(self.time_data) > self.config.max_plot_points:
self.time_data.pop(0)
for name in self.joint_names:
@@ -284,8 +378,6 @@ def init_plot(self):
self.ax.legend(loc='upper right', fontsize=10)
self.ax.grid(True, alpha=0.3)
-
- # 设置y轴范围
self.ax.set_ylim(-2, 2)
plt.tight_layout()
@@ -294,12 +386,10 @@ def init_plot(self):
def update_plot(self, frame):
"""更新绘图（动画回调函数）"""
with data_lock:
- # 更新每条线的数据
for name, line in self.lines.items():
if len(self.plot_data[name]) > 0 and len(self.time_data) == len(self.plot_data[name]):
line.set_data(self.time_data, self.plot_data[name])
- # 自动调整x轴范围
if len(self.time_data) > 0:
self.ax.set_xlim(max(0, self.time_data[-1] - 10), self.time_data[-1] + 1)
@@ -319,13 +409,23 @@ def print_robot_state(self):
self.fps = self.frame_count / elapsed_time
if current_time - self.last_print_time >= self.config.state_print_interval:
- print(f"\n===== 机器人状态（时间：{current_time:.2f}s | 帧率：{self.fps:.1f} FPS）=====")
+ print(
+ f"\n===== 机器人状态（时间：{current_time:.2f}s | 帧率：{self.fps:.1f} FPS | 模式：{self.current_mode}）=====")
for name in self.joint_names:
ctrl_id = self.joint_ctrl_ids[name]
qpos_idx = self.joint_qpos_indices[name]
- if ctrl_id != -1 and qpos_idx != -1 and qpos_idx < len(self.data.qpos):
+
+ if ctrl_id != -1 and qpos_idx != -1 and qpos_idx < len(self.data.qpos) and ctrl_id < len(
+ self.data.ctrl):
print(
f"关节 {name}: 位置 = {self.data.qpos[qpos_idx]:.2f} rad, 控制信号 = {self.data.ctrl[ctrl_id]:.2f}")
+ elif ctrl_id == -1:
+ print(f"关节 {name}: 无控制ID")
+ elif qpos_idx == -1:
+ print(f"关节 {name}: 无位置索引")
+ else:
+ print(f"关节 {name}: 索引超出范围")
+
self.last_print_time = current_time
def reset_robot(self):
@@ -333,50 +433,69 @@ def reset_robot(self):
with data_lock:
mujoco.mj_resetData(self.model, self.data)
self.data.qpos[0:7] = [0, 0, 1.0, 1, 0, 0, 0]
- # 重置控制信号缓存
+
+ # 重置控制信号和动作状态
for name in self.joint_names:
self.last_ctrl_signals[name] = 0.0
+ ctrl_id = self.joint_ctrl_ids[name]
+ if ctrl_id != -1 and ctrl_id < len(self.data.ctrl):
+ self.data.ctrl[ctrl_id] = 0.0
+
+ self.walk_phase = 0.0
+ self.wave_arm = "right"
+
# 清空绘图数据
self.plot_data = {name: [] for name in self.joint_names}
self.time_data = []
self.frame_counter = 0
- print("\n🔄 机器人已重置到初始状态！")
- def input_listener(self):
- """后台线程：监听控制台输入"""
- global sim_running
- self.input_thread_running = True
- timeout = 0.1
+ print("\n🔄 机器人已重置到初始状态！")
- while self.input_thread_running and sim_running:
+ def check_user_input(self):
+ """检查用户输入（Windows兼容）"""
+ if sys.platform == 'win32':
try:
- ready, _, _ = select.select([sys.stdin], [], [], timeout)
- if ready:
+ import msvcrt
+ if msvcrt.kbhit():
user_input = sys.stdin.readline().strip().lower()
- if user_input == 'r':
- self.reset_robot()
- elif user_input in ["sin", "random", "stop"]:
- self.current_mode = user_input
- print(f"\n🔄 运动模式已切换为：{user_input}")
- elif user_input == 'q':
- sim_running = False
- print("\n📤 收到退出指令，仿真将结束...")
- elif user_input == 'clear':
- with data_lock:
- self.plot_data = {name: [] for name in self.joint_names}
- self.time_data = []
- print("\n🧹 绘图数据已清空！")
- elif user_input:
- print(f"\n❓ 未知指令：{user_input}，支持的指令：")
- print(" - r：重置机器人")
- print(" - sin/random/stop：切换运动模式")
- print(" - clear：清空绘图数据")
- print(" - q：退出仿真")
- except Exception as e:
- print(f"\n⚠️ 输入处理失败：{e}")
- break
+ return user_input
+ except:
+ return None
+ return None
+
+ def process_user_input(self, user_input):
+ """处理用户输入指令"""
+ if not user_input:
+ return
- print("\n🔌 输入监听线程已优雅退出")
+ if user_input == 'r':
+ self.reset_robot()
+ elif user_input in ["walk", "wave", "sin", "random", "stop"]:
+ self.current_mode = user_input
+ print(f"\n🔄 运动模式已切换为：{user_input}")
+ if user_input == "walk":
+ print("👣 行走模式：机器人将进行自然行走动作")
+ elif user_input == "wave":
+ print("✋ 挥手模式：机器人将交替挥动手臂")
+ elif user_input == 'q':
+ global sim_running
+ sim_running = False
+ print("\n📤 收到退出指令，仿真将结束...")
+ elif user_input == 'clear':
+ with data_lock:
+ self.plot_data = {name: [] for name in self.joint_names}
+ self.time_data = []
+ print("\n🧹 绘图数据已清空！")
+ elif user_input:
+ print(f"\n❓ 未知指令：{user_input}，支持的指令：")
+ print(" - r：重置机器人")
+ print(" - walk：行走模式（新增）")
+ print(" - wave：挥手模式（新增）")
+ print(" - sin：正弦运动模式")
+ print(" - random：随机运动模式")
+ print(" - stop：停止运动")
+ print(" - clear：清空绘图数据")
+ print(" - q：退出仿真")
def run_simulation(self):
"""运行仿真主循环"""
@@ -385,14 +504,10 @@ def run_simulation(self):
# 初始化绘图
self.init_plot()
- # 启动输入监听线程
- input_thread = threading.Thread(target=self.input_listener)
- input_thread.start()
-
# 启动可视化动画
self.ani = FuncAnimation(self.fig, self.update_plot, interval=50, blit=True, cache_frame_data=False)
- # 显示绘图窗口（非阻塞）
+ # 显示绘图窗口
plt.show(block=False)
# 启动MuJoCo可视化
@@ -408,18 +523,27 @@ def run_simulation(self):
# 打印操作提示
print("\n📌 仿真操作提示：")
print(" - 输入 'r' 回车：重置机器人")
- print(" - 输入 'sin'/'random'/'stop' 回车：切换运动模式")
+ print(" - 输入 'walk' 回车：行走模式（新增）")
+ print(" - 输入 'wave' 回车：挥手模式（新增）")
+ print(" - 输入 'sin' 回车：正弦运动模式")
+ print(" - 输入 'random' 回车：随机运动模式")
+ print(" - 输入 'stop' 回车：停止运动")
print(" - 输入 'clear' 回车：清空绘图数据")
print(" - 输入 'q' 回车：退出仿真")
print(" - 按 Ctrl+C：强制退出仿真")
- print("\n🚀 仿真开始...")
+ print("\n🚀 仿真开始（默认模式：行走）...")
# 仿真主循环
- global sim_running
last_step_time = time.perf_counter()
while sim_running and v.is_running():
current_time = time.perf_counter()
+
+ # 检查并处理用户输入
+ user_input = self.check_user_input()
+ if user_input:
+ self.process_user_input(user_input)
+
if current_time - last_step_time >= self.config.timestep:
# 更新关节控制
self.update_joint_controls()
@@ -445,28 +569,45 @@ def run_simulation(self):
# 处理matplotlib事件
plt.pause(0.001)
- # 停止输入监听线程
- self.input_thread_running = False
- input_thread.join(timeout=1.0)
-
- # 关闭绘图窗口
+ # 清理资源
plt.close(self.fig)
-
print("\n🏁 仿真结束！")
# ====================== 程序入口 ======================
if __name__ == "__main__":
- # 设置matplotlib后端（避免显示问题）
+ # 设置matplotlib后端
import matplotlib
matplotlib.use('TkAgg')
+ # Windows控制台编码修复
+ if sys.platform == 'win32':
+ try:
+ # 设置控制台编码为UTF-8
+ import subprocess
+
+ subprocess.call('chcp 65001', shell=True)
+ except:
+ pass
+
# 初始化配置
config = SimConfig()
# 创建仿真器并运行
simulator = HumanoidSimulator(config)
- simulator.run_simulation()
- sys.exit(0)
\ No newline at end of file
+ try:
+ simulator.run_simulation()
+ except KeyboardInterrupt:
+ sim_running = False
+ print("\n⚠️ 用户中断，正在退出...")
+ except Exception as e:
+ print(f"\n❌ 程序异常：{e}")
+ import traceback
+
+ traceback.print_exc()
+ finally:
+ # 确保资源正确释放
+ plt.close('all')
+ sys.exit(0)