diff --git a/.gitignore b/.gitignore
deleted file mode 100644
index 63d50ff1bb..0000000000
--- a/.gitignore
+++ /dev/null
@@ -1,17 +0,0 @@
-# Python»º´棨ºËÐ쬽â¾ö»º´æÎļþÎÊÌ⣩
-__pycache__/
-*.pyc
-*.pyo
-*.pyd
-.pytest_cache/
-__pycache__/
-
-# ϵͳ/±༭Æ÷»º´æ
-.DS_Store
-Thumbs.db
-.vscode/
-.idea/
-
-# ÆäËû»º´棨°´Ð貹³䣩
-*.log
-*.tmp
diff --git a/src/embodied_robot/README.md b/src/embodied_robot/README.md
index 2d82baa853..3a750c11d9 100644
--- a/src/embodied_robot/README.md
+++ b/src/embodied_robot/README.md
@@ -1,16 +1,17 @@
-具身机器人:
+Embodied Robot:
-1. 行走:
- # 核心库(必须安装)
- mujoco==3.3.6
- numpy==2.2.6
+Walking:
+Core Libraries (Mandatory Installation)
+mujoco==3.3.6
+numpy==2.2.6
+python==3.12
-# 间接依赖(通常自动安装,若缺失则补充)
- absl-py==2.3.1
- glfw==2.10.0
- PyOpenGL==3.1.10
- etils==1.13.0
- importlib_resources==6.5.2
- typing_extensions==4.15.0
- zipp==3.23.0
- fsspec==2025.9.0
+Indirect Dependencies (Typically Installed Automatically; Install Manually if Missing)
+absl-py==2.3.1
+glfw==2.10.0
+PyOpenGL==3.1.10
+etils==1.13.0
+importlib_resources==6.5.2
+typing_extensions==4.15.0
+zipp==3.23.0
+fsspec==2025.9.0
diff --git a/src/embodied_robot/main.py b/src/embodied_robot/main.py
index 6a798d96e8..759bb6b642 100644
--- a/src/embodied_robot/main.py
+++ b/src/embodied_robot/main.py
@@ -1,84 +1,210 @@
-import mujoco
-import time
-from mujoco import viewer
+# -*- coding: utf-8 -*- # 强制声明文件编码为UTF-8
+# 或简化版:
+# coding=utf-8
+import os
+import sys
+import subprocess
+import platform
+from pathlib import Path
-def main():
- # 定义简单的双轮机器人模型
- robot_xml = """
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+def setup_environment():
+ """
+ Initialize runtime environment - adapt to directory structure:
+ main.py is at the same level as robot_walk, scripts/models in robot_walk subdirectory
+ """
+ # Get project root (directory of main.py)
+ project_root = Path(__file__).resolve().parent
+ print(f"📁 Project root directory: {project_root}")
+
+ # Define paths
+ robot_walk_dir = project_root / "robot_walk"
+ script_file = robot_walk_dir / "move_straight.py"
+ model_file = robot_walk_dir / "Robot_move_straight.xml"
+
+ # Check subdirectory existence
+ if not robot_walk_dir.exists():
+ print(f"\n❌ Missing subdirectory: {robot_walk_dir}")
+ print("📋 Required directory structure:")
+ print(" embodied_robot/")
+ print(" ├── main.py")
+ print(" └── robot_walk/")
+ print(" ├── move_straight.py")
+ print(" └── Robot_move_straight.xml")
+ sys.exit(1)
+ print(f"✅ Found subdirectory: {robot_walk_dir}")
+
+ # Check file existence
+ files_to_check = [
+ ("Robot control script", script_file),
+ ("Mujoco model file", model_file)
+ ]
+
+ missing_files = []
+ for file_desc, file_path in files_to_check:
+ if not file_path.exists():
+ missing_files.append(f"{file_desc}: {file_path}")
+ else:
+ print(f"✅ {file_desc} found: {file_path}")
+
+ # Handle missing files
+ if missing_files:
+ print("\n❌ Missing required files:")
+ for missing in missing_files:
+ print(f" - {missing}")
+ print("\n📋 Ensure robot_walk directory contains:")
+ print(" 1. move_straight.py (Robot control script)")
+ print(" 2. Robot_move_straight.xml (Mujoco model file)")
+ sys.exit(1)
+
+ return project_root, robot_walk_dir, script_file, model_file
+
+
+def get_python_executable():
+ """
+ Get correct Python interpreter path (priority to virtual environment)
+ """
+ python_exe = sys.executable
+ print(f"\n🐍 Using Python interpreter: {python_exe}")
+
+ # Verify Python version
+ try:
+ version_result = subprocess.run(
+ [python_exe, "--version"],
+ capture_output=True,
+ text=True,
+ check=True
+ )
+ python_version = version_result.stdout.strip()
+ print(f"🔍 Python version: {python_version}")
+
+ # Check minimum version (3.8+)
+ version_parts = python_version.split()[1].split('.')
+ major = int(version_parts[0])
+ minor = int(version_parts[1])
+ if major < 3 or (major == 3 and minor < 8):
+ print("⚠️ Warning: Mujoco recommends Python 3.8+, compatibility issues may occur")
+ except Exception as e:
+ print(f"⚠️ Failed to detect Python version: {e}")
+
+ return python_exe
+
+
+def check_dependencies():
+ """
+ Check required packages installation
+ """
+ required_packages = [
+ "mujoco",
+ "numpy"
+ ]
+
+ missing_packages = []
+ for pkg in required_packages:
+ try:
+ __import__(pkg)
+ print(f"✅ Package {pkg} is installed")
+ except ImportError:
+ missing_packages.append(pkg)
+
+ if missing_packages:
+ print("\n❌ Missing required packages:")
+ for pkg in missing_packages:
+ print(f" - {pkg}")
+ print("\n📦 Install missing packages with:")
+ print(f" {sys.executable} -m pip install {' '.join(missing_packages)}")
+
+ # Ask for auto-install
+ if input("\n📥 Auto-install missing packages? (y/n): ").lower() == 'y':
+ try:
+ subprocess.run(
+ [sys.executable, "-m", "pip", "install"] + missing_packages,
+ check=True
+ )
+ print("✅ Packages installed successfully")
+ except subprocess.CalledProcessError as e:
+ print(f"❌ Package installation failed: {e}")
+ sys.exit(1)
+
+
+def run_robot_simulation(python_exe, robot_walk_dir, script_file):
"""
+ Launch robot simulation script (run in robot_walk directory for correct path resolution)
+ """
+ print("\n🚀 Starting robot patrol simulation...")
+ print("=" * 50)
+
+ try:
+ # Set environment variables (no logs)
+ env = os.environ.copy()
+ env['MUJOCO_QUIET'] = '1'
+ env['PYTHONPATH'] = str(Path(__file__).resolve().parent) + os.pathsep + env.get('PYTHONPATH', '')
+
+ # Run script in robot_walk directory
+ result = subprocess.run(
+ [python_exe, str(script_file)],
+ cwd=str(robot_walk_dir),
+ env=env,
+ stdout=sys.stdout,
+ stderr=sys.stderr,
+ check=True
+ )
+
+ print("=" * 50)
+ print("🏁 Simulation completed successfully")
+ return result.returncode
+
+ except subprocess.CalledProcessError as e:
+ print(f"\n❌ Simulation error, return code: {e.returncode}")
+ return e.returncode
+ except KeyboardInterrupt:
+ print("\n🛑 Simulation interrupted by user")
+ return 0
+ except Exception as e:
+ print(f"\n❌ Unexpected error: {e}")
+ import traceback
+ traceback.print_exc()
+ return 1
- # 加载模型
- model = mujoco.MjModel.from_xml_string(robot_xml)
- data = mujoco.MjData(model)
- # 初始化可视化器
- viewer_instance = viewer.launch_passive(model, data)
+def main():
+ """
+ Main launcher function
+ """
- # 设置直行速度(左右轮相同速度)
- forward_speed = 1.0
- data.ctrl[0] = forward_speed # 左轮速度
- data.ctrl[1] = forward_speed # 右轮速度
+ print("=" * 50)
+ print("🤖 DeepMind Humanoid Robot Simulation Launcher")
+ print("📌 Multi-target patrol + Dynamic obstacle avoidance")
+ print("=" * 50)
- # 运行仿真10秒
- total_time = 10.0
- steps = int(total_time / model.opt.timestep)
+ # 1. Setup environment
+ try:
+ project_root, robot_walk_dir, script_file, model_file = setup_environment()
+ except Exception as e:
+ print(f"\n❌ Environment initialization failed: {e}")
+ sys.exit(1)
- start_time = time.time()
- for _ in range(steps):
- # 执行仿真步
- mujoco.mj_step(model, data)
+ # 2. Get Python executable
+ python_exe = get_python_executable()
- # 更新可视化
- viewer_instance.sync()
+ # 3. Check dependencies
+ print("\n🔍 Checking dependencies...")
+ check_dependencies()
- # 维持实时速率
- elapsed = time.time() - start_time
- expected = _ * model.opt.timestep
- if elapsed < expected:
- time.sleep(expected - elapsed)
+ # 4. Run simulation
+ exit_code = run_robot_simulation(python_exe, robot_walk_dir, script_file)
- print(f"仿真完成,机器人最终位置: {data.body('base').xpos}")
- viewer_instance.close()
+ # 5. Exit
+ sys.exit(exit_code)
if __name__ == "__main__":
- main()
-
-
-
+ # Fix Windows encoding issues
+ if platform.system() == "Windows":
+ try:
+ os.system("chcp 65001 > nul")
+ except:
+ pass
+
+ # Launch main program
+ main()
\ No newline at end of file
diff --git a/src/embodied_robot/robot_walk/Robot_move_straight.xml b/src/embodied_robot/robot_walk/Robot_move_straight.xml
index aa65f0e3a4..585472c4b8 100644
--- a/src/embodied_robot/robot_walk/Robot_move_straight.xml
+++ b/src/embodied_robot/robot_walk/Robot_move_straight.xml
@@ -1,333 +1,324 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/embodied_robot/robot_walk/move_straight.py b/src/embodied_robot/robot_walk/move_straight.py
index 960017719d..3d5504de77 100644
--- a/src/embodied_robot/robot_walk/move_straight.py
+++ b/src/embodied_robot/robot_walk/move_straight.py
@@ -1,521 +1,626 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+DeepMind Humanoid Robot Simulation
+Dynamic Obstacle Avoidance + Moving Target Tracking
+UTF-8 encoded, GitHub compatible
+"""
+
import mujoco
from mujoco import viewer
import time
import numpy as np
import random
from collections import deque
+import os
+import sys
+
+# ====================== Global Configuration ======================
+# Disable all log output (cross-version compatible)
+os.environ['MUJOCO_QUIET'] = '1'
+os.environ['MUJOCO_GL'] = 'egl'
+os.environ['PYTHONIOENCODING'] = 'utf-8'
-# 设置随机种子保证可复现
+# Set random seed for reproducibility
np.random.seed(42)
random.seed(42)
-def control_robot(model_path):
- """
- 控制DeepMind Humanoid模型:复杂动态环境下的目标导航
- 特性:多动态障碍(正弦组合/随机游走/圆周运动)+ 多障碍优先级避障 + 目标导航
- """
- # 加载模型和数据
- model = mujoco.MjModel.from_xml_path(model_path)
- data = mujoco.MjData(model)
-
- # -------------------------- 核心配置:固定导航目标点 --------------------------
- TARGET_POS = np.array([12.0, 0.0]) # 目标点后移至12米,适配更多障碍
- target_reached_threshold = 0.8 # 增大到达阈值,适配复杂环境
- navigation_target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "navigation_target")
-
- # -------------------------- 多动态障碍初始化 --------------------------
- # 障碍1(wall2):正弦组合运动(Y+Z轴)
- wall2_joint_ids = {
- "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_y"),
- "z": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_z")
- }
- wall2_motor_ids = {
- "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_y"),
- "z": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_z")
- }
- wall2_params = {
- "y_amp": 2.5, "y_freq": 0.7, "y_phase": random.uniform(0, 2 * np.pi),
- "z_amp": 0.4, "z_freq": 0.3, "z_phase": random.uniform(0, 2 * np.pi)
- }
-
- # 障碍2(wall3):随机游走运动(X+Y轴)
- wall3_joint_ids = {
- "x": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_x"),
- "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_y")
- }
- wall3_motor_ids = {
- "x": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_x"),
- "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_y")
- }
- wall3_params = {
- "x_base": 6.0, "x_range": 1.0, "x_speed": 0.4,
- "y_base": 0.0, "y_range": 2.0, "y_speed": 0.6,
- "x_dir": random.choice([-1, 1]), "y_dir": random.choice([-1, 1]),
- "x_switch": random.uniform(2.0, 4.0), "y_switch": random.uniform(1.5, 3.5)
- }
- wall3_last_switch = {"x": 0.0, "y": 0.0}
-
- # 障碍3(wall4):圆周运动(旋转+径向)
- wall4_joint_ids = {
- "rot": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_rotate"),
- "rad": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_radial")
- }
- wall4_motor_ids = {
- "rot": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rot"),
- "rad": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rad")
- }
- wall4_params = {
- "rot_speed": 0.5, "rot_dir": random.choice([-1, 1]),
- "rad_amp": 0.8, "rad_freq": 0.2, "rad_phase": random.uniform(0, 2 * np.pi),
- "rad_base": 1.2
- }
-
- # 打印障碍配置
- print("=" * 60)
- print("复杂动态环境配置:")
- print(f"• 导航目标点:({TARGET_POS[0]}, {TARGET_POS[1]}),到达阈值:{target_reached_threshold}m")
- print(f"• 动态障碍1(wall2):Y轴正弦(振幅{wall2_params['y_amp']}, 频率{wall2_params['y_freq']}) + Z轴正弦")
- print(f"• 动态障碍2(wall3):X/Y轴随机游走(速度{wall3_params['x_speed']}, {wall3_params['y_speed']})")
- print(f"• 动态障碍3(wall4):圆周运动(转速{wall4_params['rot_speed']}, 径向振幅{wall4_params['rad_amp']})")
- print("=" * 60)
-
- # -------------------------- 障碍检测初始化(修复核心问题) --------------------------
- # 只检测真正的障碍本体(wall1, wall2, wall3, wall4),排除base/rot_base等基座
- valid_wall_names = ["wall1", "wall2", "wall3", "wall4"]
- wall_ids = []
- wall_names = []
- wall_types = {} # 标记障碍类型:fixed/dynamic1/dynamic2/dynamic3
-
- for wall_name in valid_wall_names:
- wall_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, wall_name)
- if wall_id != -1:
- wall_ids.append(wall_id)
- wall_names.append(wall_name)
- # 分配障碍类型
- if wall_name == "wall1":
- wall_types[wall_name] = "fixed"
- elif wall_name == "wall2":
- wall_types[wall_name] = "dynamic1"
- elif wall_name == "wall3":
- wall_types[wall_name] = "dynamic2"
- elif wall_name == "wall4":
- wall_types[wall_name] = "dynamic3"
-
- torso_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "torso")
- print(f"检测到有效障碍:{[(name, wall_types[name]) for name in wall_names]}")
-
- # -------------------------- 核心参数配置 --------------------------
- # 多障碍避障参数
- avoid_obstacle = False
- obstacle_distance_threshold = 2.0 # 增大避障触发距离
- obstacle_avoidance_time = 0
- obstacle_avoidance_duration = 5.0 # 延长避障时间
- turn_direction = 0
- return_to_path = False
- return_time = 0
- return_duration = 4.0
- stop_walk = False
- target_reached = False
- closest_wall_id = -1
- closest_wall_type = ""
- closest_wall_name = "" # 初始化closest_wall_name
- dir_name = "" # 初始化dir_name
- # 障碍位置缓存(用于运动预判)
- wall_pos_history = {name: deque(maxlen=10) for name in wall_names}
-
- # 步态控制参数(降低速度,增强稳定性)
- gait_period = 2.2
- swing_gain = 0.8
- stance_gain = 0.75
- forward_speed = 0.3
- heading_kp = 90.0 # 增大朝向增益
-
- # 姿态稳定参数
- torso_pitch_target = 0.0
- torso_roll_target = 0.0
- balance_kp = 110.0
- balance_kd = 18.0
-
- # 启动可视化器
- mujoco.set_mjcb_control(None)
- with viewer.launch_passive(model, data) as viewer_instance:
- print("\nDeepMind Humanoid复杂动态环境导航仿真启动!")
- print(f"导航逻辑:向目标点({TARGET_POS[0]},{TARGET_POS[1]})移动 → 多障碍检测 → 优先级避障 → 回归路径 → 到达目标")
- start_time = time.time()
-
- try:
- while True:
- if not viewer_instance.is_running():
- break
-
- elapsed_time = time.time() - start_time
-
- # -------------------------- 1. 多动态障碍运动控制 --------------------------
- # 障碍1(wall2):Y+Z轴正弦组合运动
- if all(id != -1 for id in wall2_motor_ids.values()):
- # Y轴:主正弦运动
- wall2_y_target = wall2_params["y_amp"] * np.sin(
- wall2_params["y_freq"] * elapsed_time + wall2_params["y_phase"])
- # Z轴:副正弦运动(垂直方向)
- wall2_z_target = wall2_params["z_amp"] * np.sin(
- wall2_params["z_freq"] * elapsed_time + wall2_params["z_phase"]) + 0.75
- # PD控制
- data.ctrl[wall2_motor_ids["y"]] = (wall2_y_target - data.qpos[wall2_joint_ids["y"]]) * 2.5
- data.ctrl[wall2_motor_ids["z"]] = (wall2_z_target - data.qpos[wall2_joint_ids["z"]]) * 1.8
-
- # 障碍2(wall3):随机游走运动
- if all(id != -1 for id in wall3_motor_ids.values()):
- # 随机切换运动方向
- if elapsed_time - wall3_last_switch["x"] > wall3_params["x_switch"]:
- wall3_params["x_dir"] *= -1
- wall3_params["x_switch"] = random.uniform(2.0, 4.0)
- wall3_last_switch["x"] = elapsed_time
- if elapsed_time - wall3_last_switch["y"] > wall3_params["y_switch"]:
- wall3_params["y_dir"] *= -1
- wall3_params["y_switch"] = random.uniform(1.5, 3.5)
- wall3_last_switch["y"] = elapsed_time
-
- # 计算目标位置
- wall3_x_target = wall3_params["x_base"] + wall3_params["x_dir"] * wall3_params["x_speed"] * (
- elapsed_time % 5)
- wall3_y_target = wall3_params["y_base"] + wall3_params["y_dir"] * wall3_params["y_speed"] * (
- elapsed_time % 4)
- # 限制范围
- wall3_x_target = np.clip(wall3_x_target, 5.0, 7.0)
- wall3_y_target = np.clip(wall3_y_target, -2.5, 2.5)
- # PD控制
- data.ctrl[wall3_motor_ids["x"]] = (wall3_x_target - data.qpos[wall3_joint_ids["x"]]) * 2.2
- data.ctrl[wall3_motor_ids["y"]] = (wall3_y_target - data.qpos[wall3_joint_ids["y"]]) * 2.0
-
- # 障碍3(wall4):圆周运动
- if all(id != -1 for id in wall4_motor_ids.values()):
- # 旋转运动
- wall4_rot_target = wall4_params["rot_dir"] * wall4_params["rot_speed"] * elapsed_time
- # 径向运动(正弦变化半径)
- wall4_rad_target = wall4_params["rad_base"] + wall4_params["rad_amp"] * np.sin(
- wall4_params["rad_freq"] * elapsed_time + wall4_params["rad_phase"])
- # PD控制
- data.ctrl[wall4_motor_ids["rot"]] = (wall4_rot_target - data.qpos[wall4_joint_ids["rot"]]) * 1.5
- data.ctrl[wall4_motor_ids["rad"]] = (wall4_rad_target - data.qpos[wall4_joint_ids["rad"]]) * 2.0
-
- # -------------------------- 2. 导航状态计算 --------------------------
- yaw_error = 0.0 # 初始化
- if torso_id != -1 and not target_reached:
- torso_pos = data.xpos[torso_id]
- robot_xy = torso_pos[:2]
- target_vector = TARGET_POS - robot_xy
- distance_to_target = np.linalg.norm(target_vector)
-
- # 检查是否到达目标点
- if distance_to_target < target_reached_threshold:
- target_reached = True
- stop_walk = True
- print(f"\n\n✅ 到达目标点!")
- print(f"最终位置:x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f}")
- print(f"目标点:x={TARGET_POS[0]}, y={TARGET_POS[1]} | 剩余距离:{distance_to_target:.2f}m")
- continue
-
- # 计算机器人朝向和目标方向
- torso_quat = data.xquat[torso_id]
- robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
- torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 + torso_quat[
- 3] ** 2)
- target_yaw = np.arctan2(target_vector[1], target_vector[0])
- yaw_error = target_yaw - robot_yaw
- yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
-
- # -------------------------- 3. 多障碍检测与优先级排序 --------------------------
- distance_to_closest_wall = float('inf')
- closest_wall_pos = np.zeros(2)
- # 障碍优先级:dynamic1(wall2)> dynamic2(wall3)> dynamic3(wall4)> fixed(wall1)
- wall_priority = {"dynamic1": 4, "dynamic2": 3, "dynamic3": 2, "fixed": 1}
-
- if wall_ids and torso_id != -1 and not stop_walk and not target_reached:
- torso_pos = data.xpos[torso_id]
- wall_distances = []
-
- for idx, wall_id in enumerate(wall_ids):
- wall_name = wall_names[idx]
- wall_type = wall_types.get(wall_name, "fixed")
-
- # 获取当前位置并缓存
- wall_pos = data.xpos[wall_id]
- wall_pos_history[wall_name].append(wall_pos[:2])
- # 计算距离
- current_distance = np.linalg.norm(torso_pos[:2] - wall_pos[:2])
-
- # 动态障碍位置预判(基于历史数据)
- predicted_distance = current_distance
- if len(wall_pos_history[wall_name]) > 5 and wall_type != "fixed":
- # 计算运动趋势
- pos_history = np.array(wall_pos_history[wall_name])
- velocity = (pos_history[-1] - pos_history[0]) / len(pos_history) * model.opt.timestep * 10
- # 预测0.8秒后的位置
- future_pos = wall_pos[:2] + velocity * 0.8
- predicted_distance = np.linalg.norm(torso_pos[:2] - future_pos)
-
- # 加入优先级权重
- priority_weight = wall_priority[wall_type]
- weighted_distance = predicted_distance / priority_weight
-
- wall_distances.append({
- "name": wall_name,
- "type": wall_type,
- "id": wall_id,
- "pos": wall_pos[:2],
- "current_dist": current_distance,
- "predicted_dist": predicted_distance,
- "weighted_dist": weighted_distance
- })
-
- # 按加权距离排序(最近的优先)
- wall_distances.sort(key=lambda x: x["weighted_dist"])
-
- if wall_distances:
- closest_wall = wall_distances[0]
- closest_wall_id = closest_wall["id"]
- closest_wall_name = closest_wall["name"]
- closest_wall_type = closest_wall["type"]
- closest_wall_pos = closest_wall["pos"]
- distance_to_closest_wall = closest_wall["predicted_dist"]
-
- # -------------------------- 4. 多障碍避障状态切换 --------------------------
- if closest_wall_id != -1 and torso_id != -1 and not stop_walk and not target_reached:
- # 触发避障
- if (distance_to_closest_wall < obstacle_distance_threshold and
- not avoid_obstacle and not return_to_path):
- avoid_obstacle = True
- obstacle_avoidance_time = time.time()
- # 智能选择转向方向:远离障碍+朝向目标
- torso_pos = data.xpos[torso_id]
- # 计算障碍相对于机器人的位置
- wall_relative = closest_wall_pos - torso_pos[:2]
- # 计算目标相对于机器人的位置
- target_relative = TARGET_POS - torso_pos[:2]
- # 叉乘判断转向方向(优先绕开障碍且朝向目标)
- cross_product = np.cross(np.append(wall_relative, 0), np.append(target_relative, 0))[2]
- turn_direction = -1 if cross_product > 0 else 1
- dir_name = "左转" if turn_direction == -1 else "右转"
-
- print(f"\n⚠️ 检测到高优先级障碍【{closest_wall_name}({closest_wall_type})】!")
- print(f" 预测距离:{distance_to_closest_wall:.2f}m | 转向方向:{dir_name}")
-
- # 避障完成,回归导航路径
- if avoid_obstacle and (time.time() - obstacle_avoidance_time) > obstacle_avoidance_duration:
- avoid_obstacle = False
- return_to_path = True
- return_time = time.time()
- print(f"✅ 避障完成,回归导航路径...")
-
- # 回归完成,继续向目标点移动
- if return_to_path and (time.time() - return_time) > return_duration:
- return_to_path = False
- print(f"✅ 回归路径完成,继续向目标点移动...")
-
- # -------------------------- 5. 步态周期计算 --------------------------
- cycle = elapsed_time % gait_period
- phase = cycle / gait_period
-
- # -------------------------- 6. 关节控制核心逻辑 --------------------------
- data.ctrl[:model.nu - 6] = 0.0 # 重置机器人控制指令(保留障碍电机)
-
- if stop_walk or target_reached:
- continue
-
- elif return_to_path:
- # 回归导航路径:精准朝向目标点
- return_phase = (time.time() - return_time) / return_duration
- return_speed = 1.5 * np.cos(return_phase * np.pi)
-
- # 朝向目标点回正
- if torso_id != -1:
- torso_quat = data.xquat[torso_id]
- robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
- torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 +
- torso_quat[3] ** 2)
- target_vector = TARGET_POS - data.xpos[torso_id][:2]
- target_yaw = np.arctan2(target_vector[1], target_vector[0])
- yaw_error = target_yaw - robot_yaw
- yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
-
- # 转向控制
- abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
- hip_z_right_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_right")
- hip_z_left_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_left")
-
- if 0 <= abdomen_z_act_id < model.nu - 6:
- data.ctrl[abdomen_z_act_id] = heading_kp * yaw_error * return_speed
- if 0 <= hip_z_right_act_id < model.nu - 6:
- data.ctrl[hip_z_right_act_id] = -yaw_error * return_speed * 0.8
- if 0 <= hip_z_left_act_id < model.nu - 6:
- data.ctrl[hip_z_left_act_id] = yaw_error * return_speed * 0.8
-
- # 稳定站立
- for side in ["right", "left"]:
- hip_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
- knee_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
- ankle_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_y_{side}")
- if 0 <= hip_y_act_id < model.nu - 6:
- data.ctrl[hip_y_act_id] = -0.8
- if 0 <= knee_act_id < model.nu - 6:
- data.ctrl[knee_act_id] = 1.1
- if 0 <= ankle_y_act_id < model.nu - 6:
- data.ctrl[ankle_y_act_id] = 0.4
-
- elif avoid_obstacle:
- # 多障碍避障模式:增强转向稳定性
- avoid_phase = (time.time() - obstacle_avoidance_time) / obstacle_avoidance_duration
- turn_speed = 1.6 * np.sin(avoid_phase * np.pi)
-
- # 转向控制
- hip_z_right_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_right")
- hip_z_left_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_left")
- abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
-
- if 0 <= hip_z_right_act_id < model.nu - 6:
- data.ctrl[hip_z_right_act_id] = turn_direction * turn_speed * 1.2
- if 0 <= hip_z_left_act_id < model.nu - 6:
- data.ctrl[hip_z_left_act_id] = -turn_direction * turn_speed * 1.2
- if 0 <= abdomen_z_act_id < model.nu - 6:
- data.ctrl[abdomen_z_act_id] = turn_direction * turn_speed * 2.0
-
- # 增强平衡控制
- for side in ["right", "left"]:
- hip_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
- knee_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
- ankle_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_x_{side}")
- if 0 <= hip_y_act_id < model.nu - 6:
- data.ctrl[hip_y_act_id] = -1.1
- if 0 <= knee_act_id < model.nu - 6:
- data.ctrl[knee_act_id] = 1.3
- if 0 <= ankle_x_act_id < model.nu - 6:
- data.ctrl[ankle_x_act_id] = 0.3
-
- else:
- # 正常导航模式:向目标点移动
- if torso_id != -1:
- torso_quat = data.xquat[torso_id]
- robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
- torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 +
- torso_quat[3] ** 2)
- target_vector = TARGET_POS - data.xpos[torso_id][:2]
- target_yaw = np.arctan2(target_vector[1], target_vector[0])
- yaw_error = target_yaw - robot_yaw
- yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
-
- # 朝向目标点的转向控制
- abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
- if 0 <= abdomen_z_act_id < model.nu - 6:
- data.ctrl[abdomen_z_act_id] = heading_kp * yaw_error * 0.12
-
- # 腿部步态控制
- for side, sign in [("right", 1), ("left", -1)]:
- swing_phase = (phase + 0.5 * sign) % 1.0
-
- # 查询电机ID
- hip_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_x_{side}")
- hip_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_z_{side}")
- hip_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
- knee_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
- ankle_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_y_{side}")
- ankle_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_x_{side}")
-
- # 腿部关节控制
- if 0 <= hip_x_act_id < model.nu - 6:
- data.ctrl[hip_x_act_id] = swing_gain * np.sin(2 * np.pi * swing_phase) * forward_speed
- if 0 <= hip_z_act_id < model.nu - 6:
- data.ctrl[hip_z_act_id] = stance_gain * np.cos(
- 2 * np.pi * swing_phase) * 0.2 + yaw_error * 0.12
- if 0 <= hip_y_act_id < model.nu - 6:
- data.ctrl[hip_y_act_id] = -0.95 * np.sin(2 * np.pi * swing_phase) - 0.45
- if 0 <= knee_act_id < model.nu - 6:
- data.ctrl[knee_act_id] = 1.25 * np.sin(2 * np.pi * swing_phase) + 0.75
- if 0 <= ankle_y_act_id < model.nu - 6:
- data.ctrl[ankle_y_act_id] = 0.35 * np.cos(2 * np.pi * swing_phase)
- if 0 <= ankle_x_act_id < model.nu - 6:
- data.ctrl[ankle_x_act_id] = 0.18 * np.sin(2 * np.pi * swing_phase)
-
- # 躯干稳定控制
- abdomen_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
- abdomen_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_y")
- abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_x")
-
- if torso_id != -1:
- pitch = 2 * (torso_quat[1] * torso_quat[3] - torso_quat[0] * torso_quat[2])
- roll = 2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3])
-
- if 0 <= abdomen_x_act_id < model.nu - 6:
- data.ctrl[abdomen_x_act_id] = balance_kp * (torso_roll_target - roll) - balance_kd * \
- data.qvel[abdomen_x_act_id]
- if 0 <= abdomen_y_act_id < model.nu - 6:
- data.ctrl[abdomen_y_act_id] = balance_kp * (torso_pitch_target - pitch) - balance_kd * \
- data.qvel[abdomen_y_act_id]
-
- # 手臂自然摆动
- for side, sign in [("right", 1), ("left", -1)]:
- shoulder1_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"shoulder1_{side}")
- shoulder2_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"shoulder2_{side}")
- elbow_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"elbow_{side}")
-
- shoulder1_cmd = 0.22 * np.sin(2 * np.pi * (phase + 0.5 * sign))
- shoulder2_cmd = 0.17 * np.cos(2 * np.pi * (phase + 0.5 * sign))
- elbow_cmd = -0.42 * np.sin(2 * np.pi * (phase + 0.5 * sign)) - 0.27
-
- if 0 <= shoulder1_act_id < model.nu - 6:
- data.ctrl[shoulder1_act_id] = shoulder1_cmd
- if 0 <= shoulder2_act_id < model.nu - 6:
- data.ctrl[shoulder2_act_id] = shoulder2_cmd
- if 0 <= elbow_act_id < model.nu - 6:
- data.ctrl[elbow_act_id] = elbow_cmd
-
- # -------------------------- 7. 仿真推进 --------------------------
- mujoco.mj_step(model, data)
- viewer_instance.sync()
-
- # -------------------------- 8. 实时状态输出 --------------------------
- if torso_id != -1 and int(elapsed_time * 1) % 1 == 0:
- if target_reached:
- status = "✅ 到达目标点"
- nav_info = f"剩余距离:0.00m"
- obstacle_info = "—"
- elif stop_walk:
- status = "已停止"
- nav_info = "—"
- obstacle_info = "—"
- else:
- if return_to_path:
- status = "回归导航路径中"
- elif avoid_obstacle:
- status = f"避障中({dir_name} | {closest_wall_name})"
- else:
- status = "向目标点移动中"
-
- # 导航信息
- distance_to_target = np.linalg.norm(TARGET_POS - data.xpos[torso_id][:2])
- nav_info = f"剩余{distance_to_target:.2f}m | 朝向误差{np.degrees(yaw_error):.1f}°"
-
- # 障碍信息
- if closest_wall_name:
- obstacle_info = f"{closest_wall_name}({closest_wall_type}):{distance_to_closest_wall:.2f}m"
- else:
- obstacle_info = "无"
-
- torso_pos = data.xpos[torso_id]
- print(
- f"\r时间:{elapsed_time:.1f}s | 位置:x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | 导航:{nav_info} | 障碍:{obstacle_info} | 状态:{status}",
- end="")
-
- time.sleep(model.opt.timestep * 2)
-
- except KeyboardInterrupt:
- print("\n\n仿真被用户中断")
- except Exception as e:
- print(f"\n\n运行错误:{e}")
- import traceback
- traceback.print_exc()
+class DynamicPatrolController:
+ def __init__(self, model_path):
+ """Initialize robot controller with dynamic target and obstacle support"""
+ # Load model and data
+ self.model = mujoco.MjModel.from_xml_path(model_path)
+ self.data = mujoco.MjData(self.model)
+
+ # -------------------------- Core Parameters --------------------------
+ # Simulation settings
+ self.sim_start_time = 0.0
+ self.last_print_time = 0.0
+ self.target_switch_cooldown = 2.0
+ self.last_target_switch_time = 0.0
+
+ # Patrol target configuration (dynamic updatable)
+ self.patrol_points = [
+ {"name": "patrol_target_1", "pos": np.array([0.0, 0.0]), "label": "Start Point", "update_interval": 8.0},
+ {"name": "patrol_target_2", "pos": np.array([4.0, -2.0]), "label": "Patrol Point 1 (SW)",
+ "update_interval": 10.0},
+ {"name": "patrol_target_3", "pos": np.array([8.0, 2.0]), "label": "Patrol Point 2 (NE)",
+ "update_interval": 12.0},
+ {"name": "patrol_target_4", "pos": np.array([10.0, -1.0]), "label": "Patrol Point 3 (NW)",
+ "update_interval": 9.0},
+ {"name": "patrol_target_5", "pos": np.array([12.0, 0.0]), "label": "Final Point", "update_interval": 11.0}
+ ]
+ self.current_target_idx = 0
+ self.patrol_cycles = 0
+ self.patrol_completed = False
+ self.target_reached_threshold = 0.8
+
+ # Dynamic target control parameters
+ self.patrol_motor_ids = {}
+ self.patrol_joint_ids = {}
+ self.patrol_body_ids = {}
+ self.last_target_update = {i: 0.0 for i in range(len(self.patrol_points))}
+ self.target_movement_range = {"x": [-2.0, 14.0], "y": [-5.0, 5.0]}
+ self.target_move_speed = 0.5
+
+ # Obstacle detection & avoidance parameters
+ self.valid_wall_names = ["wall1", "wall2", "wall3", "wall4"]
+ self.wall_ids = []
+ self.wall_names = []
+ self.wall_types = {}
+ self.wall_pos_history = {}
+ self.obstacle_distance_threshold = 2.0
+ self.obstacle_avoidance_duration = 5.0
+ self.return_to_path_duration = 4.0
+ self.wall_priority = {"dynamic1": 4, "dynamic2": 3, "dynamic3": 2, "fixed": 1}
+
+ # Avoidance state variables
+ self.avoid_obstacle = False
+ self.return_to_path = False
+ self.obstacle_avoidance_start = 0.0
+ self.return_to_path_start = 0.0
+ self.turn_direction = 0 # -1 = left, 1 = right
+ self.closest_wall_info = {"name": "", "distance": float('inf'), "type": ""}
+ self.turn_dir_label = ""
+
+ # Robot control parameters
+ self.gait_period = 2.2
+ self.swing_gain = 0.8
+ self.stance_gain = 0.75
+ self.forward_speed = 0.3
+ self.heading_kp = 90.0
+ self.balance_kp = 110.0
+ self.balance_kd = 18.0
+ self.torso_pitch_target = 0.0
+ self.torso_roll_target = 0.0
+
+ # Initialize component IDs
+ self._init_component_ids()
+ # Initialize obstacle position history
+ self._init_obstacle_history()
+
+ def _init_component_ids(self):
+ """Initialize all mujoco component IDs (joints, actuators, bodies)"""
+ # Torso ID (core robot body)
+ self.torso_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "torso")
+
+ # Dynamic patrol target IDs
+ for idx, point in enumerate(self.patrol_points):
+ # Motor (actuator) IDs
+ self.patrol_motor_ids[idx] = {
+ "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"patrol{idx + 1}_motor_x"),
+ "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"patrol{idx + 1}_motor_y")
+ }
+ # Joint IDs (slide joints for movement)
+ self.patrol_joint_ids[idx] = {
+ "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, f"patrol{idx + 1}_slide_x"),
+ "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, f"patrol{idx + 1}_slide_y")
+ }
+ # Body IDs (target markers)
+ self.patrol_body_ids[idx] = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, point["name"])
+
+ # Dynamic obstacle IDs
+ # Wall2 (sinusoidal motion)
+ self.wall2_joint_ids = {
+ "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_y"),
+ "z": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_z")
+ }
+ self.wall2_motor_ids = {
+ "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_y"),
+ "z": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_z")
+ }
+ self.wall2_params = {
+ "y_amp": 2.5, "y_freq": 0.7, "y_phase": random.uniform(0, 2 * np.pi),
+ "z_amp": 0.4, "z_freq": 0.3, "z_phase": random.uniform(0, 2 * np.pi)
+ }
+
+ # Wall3 (random walk)
+ self.wall3_joint_ids = {
+ "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_x"),
+ "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_y")
+ }
+ self.wall3_motor_ids = {
+ "x": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_x"),
+ "y": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_y")
+ }
+ self.wall3_params = {
+ "x_base": 6.0, "x_range": 1.0, "x_speed": 0.4,
+ "y_base": 0.0, "y_range": 2.0, "y_speed": 0.6,
+ "x_dir": random.choice([-1, 1]), "y_dir": random.choice([-1, 1]),
+ "x_switch": random.uniform(2.0, 4.0), "y_switch": random.uniform(1.5, 3.5)
+ }
+ self.wall3_last_switch = {"x": 0.0, "y": 0.0}
+
+ # Wall4 (circular motion)
+ self.wall4_joint_ids = {
+ "rot": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_rotate"),
+ "rad": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_radial")
+ }
+ self.wall4_motor_ids = {
+ "rot": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rot"),
+ "rad": mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rad")
+ }
+ self.wall4_params = {
+ "rot_speed": 0.5, "rot_dir": random.choice([-1, 1]),
+ "rad_amp": 0.8, "rad_freq": 0.2, "rad_phase": random.uniform(0, 2 * np.pi),
+ "rad_base": 1.2
+ }
+
+ # Wall IDs for obstacle detection
+ for wall_name in self.valid_wall_names:
+ wall_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, wall_name)
+ if wall_id != -1:
+ self.wall_ids.append(wall_id)
+ self.wall_names.append(wall_name)
+ if wall_name == "wall1":
+ self.wall_types[wall_name] = "fixed"
+ elif wall_name == "wall2":
+ self.wall_types[wall_name] = "dynamic1"
+ elif wall_name == "wall3":
+ self.wall_types[wall_name] = "dynamic2"
+ elif wall_name == "wall4":
+ self.wall_types[wall_name] = "dynamic3"
+
+ def _init_obstacle_history(self):
+ """Initialize obstacle position history for prediction"""
+ for wall_name in self.wall_names:
+ self.wall_pos_history[wall_name] = deque(maxlen=10)
+
+ def _update_dynamic_targets(self, elapsed_time):
+ """Update dynamic patrol target positions in real-time"""
+ current_target = self.patrol_points[self.current_target_idx]
+
+ # 1. Real-time sync with target body positions (from XML)
+ for idx in self.patrol_body_ids:
+ if self.patrol_body_ids[idx] != -1:
+ target_body_pos = self.data.xpos[self.patrol_body_ids[idx]]
+ self.patrol_points[idx]["pos"] = np.array([target_body_pos[0], target_body_pos[1]])
+
+ # 2. Randomly move targets at specified intervals
+ for idx, point in enumerate(self.patrol_points):
+ if (elapsed_time - self.last_target_update[idx] > point["update_interval"] and
+ not self.avoid_obstacle and not self.return_to_path):
+
+ # Generate new random position (within safe range)
+ new_x = random.uniform(self.target_movement_range["x"][0], self.target_movement_range["x"][1])
+ new_y = random.uniform(self.target_movement_range["y"][0], self.target_movement_range["y"][1])
+
+ # Avoid obstacle areas
+ if self.torso_id != -1:
+ torso_pos = self.data.xpos[self.torso_id][:2]
+ while np.linalg.norm(np.array([new_x, new_y]) - torso_pos) < 3.0:
+ new_x = random.uniform(self.target_movement_range["x"][0], self.target_movement_range["x"][1])
+ new_y = random.uniform(self.target_movement_range["y"][0], self.target_movement_range["y"][1])
+
+ # Control target movement via actuators
+ if (self.patrol_motor_ids[idx]["x"] != -1 and
+ self.patrol_motor_ids[idx]["y"] != -1 and
+ self.patrol_joint_ids[idx]["x"] != -1 and
+ self.patrol_joint_ids[idx]["y"] != -1):
+ # Smooth movement control
+ current_x = self.data.qpos[self.patrol_joint_ids[idx]["x"]]
+ current_y = self.data.qpos[self.patrol_joint_ids[idx]["y"]]
+
+ self.data.ctrl[self.patrol_motor_ids[idx]["x"]] = (new_x - current_x) * self.target_move_speed
+ self.data.ctrl[self.patrol_motor_ids[idx]["y"]] = (new_y - current_y) * self.target_move_speed
+
+ # Update target position and timestamp
+ self.patrol_points[idx]["pos"] = np.array([new_x, new_y])
+ self.last_target_update[idx] = elapsed_time
+
+ if idx == self.current_target_idx:
+ print(f"\n🔄 Target updated: {point['label']} moved to ({new_x:.2f}, {new_y:.2f})")
+
+ def _control_dynamic_obstacles(self, elapsed_time):
+ """Control dynamic obstacle movements"""
+ # Wall2: Sinusoidal Y+Z motion
+ if all(id != -1 for id in self.wall2_motor_ids.values()):
+ wall2_y_target = self.wall2_params["y_amp"] * np.sin(
+ self.wall2_params["y_freq"] * elapsed_time + self.wall2_params["y_phase"])
+ wall2_z_target = self.wall2_params["z_amp"] * np.sin(
+ self.wall2_params["z_freq"] * elapsed_time + self.wall2_params["z_phase"]) + 0.75
+ self.data.ctrl[self.wall2_motor_ids["y"]] = (wall2_y_target - self.data.qpos[
+ self.wall2_joint_ids["y"]]) * 2.5
+ self.data.ctrl[self.wall2_motor_ids["z"]] = (wall2_z_target - self.data.qpos[
+ self.wall2_joint_ids["z"]]) * 1.8
+
+ # Wall3: Random walk X+Y
+ if all(id != -1 for id in self.wall3_motor_ids.values()):
+ if elapsed_time - self.wall3_last_switch["x"] > self.wall3_params["x_switch"]:
+ self.wall3_params["x_dir"] *= -1
+ self.wall3_params["x_switch"] = random.uniform(2.0, 4.0)
+ self.wall3_last_switch["x"] = elapsed_time
+
+ if elapsed_time - self.wall3_last_switch["y"] > self.wall3_params["y_switch"]:
+ self.wall3_params["y_dir"] *= -1
+ self.wall3_params["y_switch"] = random.uniform(1.5, 3.5)
+ self.wall3_last_switch["y"] = elapsed_time
+
+ wall3_x_target = self.wall3_params["x_base"] + self.wall3_params["x_dir"] * self.wall3_params["x_speed"] * (
+ elapsed_time % 5)
+ wall3_y_target = self.wall3_params["y_base"] + self.wall3_params["y_dir"] * self.wall3_params["y_speed"] * (
+ elapsed_time % 4)
+ wall3_x_target = np.clip(wall3_x_target, 5.0, 7.0)
+ wall3_y_target = np.clip(wall3_y_target, -2.5, 2.5)
+
+ self.data.ctrl[self.wall3_motor_ids["x"]] = (wall3_x_target - self.data.qpos[
+ self.wall3_joint_ids["x"]]) * 2.2
+ self.data.ctrl[self.wall3_motor_ids["y"]] = (wall3_y_target - self.data.qpos[
+ self.wall3_joint_ids["y"]]) * 2.0
+
+ # Wall4: Circular motion
+ if all(id != -1 for id in self.wall4_motor_ids.values()):
+ wall4_rot_target = self.wall4_params["rot_dir"] * self.wall4_params["rot_speed"] * elapsed_time
+ wall4_rad_target = self.wall4_params["rad_base"] + self.wall4_params["rad_amp"] * np.sin(
+ self.wall4_params["rad_freq"] * elapsed_time + self.wall4_params["rad_phase"])
+
+ self.data.ctrl[self.wall4_motor_ids["rot"]] = (wall4_rot_target - self.data.qpos[
+ self.wall4_joint_ids["rot"]]) * 1.5
+ self.data.ctrl[self.wall4_motor_ids["rad"]] = (wall4_rad_target - self.data.qpos[
+ self.wall4_joint_ids["rad"]]) * 2.0
+
+ def _detect_obstacles(self, elapsed_time):
+ """Detect obstacles and determine avoidance strategy"""
+ if not self.wall_ids or self.torso_id == -1 or self.patrol_completed:
+ return
+
+ torso_pos = self.data.xpos[self.torso_id][:2]
+ wall_distances = []
+
+ # Calculate obstacle distances (with prediction for dynamic obstacles)
+ for idx, wall_id in enumerate(self.wall_ids):
+ wall_name = self.wall_names[idx]
+ wall_type = self.wall_types.get(wall_name, "fixed")
+
+ wall_pos = self.data.xpos[wall_id][:2]
+ self.wall_pos_history[wall_name].append(wall_pos)
+ current_distance = np.linalg.norm(torso_pos - wall_pos)
+
+ # Predict future position for dynamic obstacles
+ predicted_distance = current_distance
+ if len(self.wall_pos_history[wall_name]) > 5 and wall_type != "fixed":
+ pos_history = np.array(self.wall_pos_history[wall_name])
+ velocity = (pos_history[-1] - pos_history[0]) / len(pos_history) * self.model.opt.timestep * 10
+ future_pos = wall_pos + velocity * 0.8
+ predicted_distance = np.linalg.norm(torso_pos - future_pos)
+
+ # Weight distance by obstacle priority (dynamic > fixed)
+ weighted_distance = predicted_distance / self.wall_priority[wall_type]
+ wall_distances.append({
+ "name": wall_name,
+ "type": wall_type,
+ "id": wall_id,
+ "pos": wall_pos,
+ "current_dist": current_distance,
+ "predicted_dist": predicted_distance,
+ "weighted_dist": weighted_distance
+ })
+
+ # Find closest obstacle (priority-based)
+ if wall_distances:
+ wall_distances.sort(key=lambda x: x["weighted_dist"])
+ closest_wall = wall_distances[0]
+
+ self.closest_wall_info = {
+ "name": closest_wall["name"],
+ "distance": closest_wall["predicted_dist"],
+ "type": closest_wall["type"],
+ "pos": closest_wall["pos"]
+ }
+
+ # Trigger obstacle avoidance
+ if (closest_wall["predicted_dist"] < self.obstacle_distance_threshold and
+ not self.avoid_obstacle and not self.return_to_path):
+ self.avoid_obstacle = True
+ self.obstacle_avoidance_start = elapsed_time
+
+ # Determine turn direction (away from obstacle)
+ wall_relative = closest_wall["pos"] - torso_pos
+ target_vector = self.patrol_points[self.current_target_idx]["pos"] - torso_pos
+ cross_product = np.cross(np.append(wall_relative, 0), np.append(target_vector, 0))[2]
+
+ self.turn_direction = -1 if cross_product > 0 else 1
+ self.turn_dir_label = "Left" if self.turn_direction == -1 else "Right"
+
+ print(
+ f"\n⚠️ Obstacle detected: {closest_wall['name']} (distance: {closest_wall['predicted_dist']:.2f}m) - Turning {self.turn_dir_label}")
+
+ # Complete avoidance: return to path
+ if self.avoid_obstacle and (
+ elapsed_time - self.obstacle_avoidance_start) > self.obstacle_avoidance_duration:
+ self.avoid_obstacle = False
+ self.return_to_path = True
+ self.return_to_path_start = elapsed_time
+ print(f"✅ Obstacle avoidance completed - returning to path")
+
+ # Return to path completed
+ if self.return_to_path and (elapsed_time - self.return_to_path_start) > self.return_to_path_duration:
+ self.return_to_path = False
+ print(
+ f"✅ Back to patrol path - tracking target: {self.patrol_points[self.current_target_idx]['label']}")
+
+ def _control_robot_gait(self, elapsed_time):
+ """Control robot gait and movement"""
+ if self.torso_id == -1 or self.patrol_completed:
+ return
+
+ current_target = self.patrol_points[self.current_target_idx]
+ torso_pos = self.data.xpos[self.torso_id][:2]
+ target_vector = current_target["pos"] - torso_pos
+ distance_to_target = np.linalg.norm(target_vector)
+
+ # Check if target is reached
+ if (distance_to_target < self.target_reached_threshold and
+ not self.patrol_completed and
+ elapsed_time - self.last_target_switch_time > self.target_switch_cooldown):
+
+ print(f"\n✅ Reached target: {current_target['label']} (x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f})")
+ self.last_target_switch_time = elapsed_time
+
+ # Switch to next target
+ if self.current_target_idx < len(self.patrol_points) - 1:
+ self.current_target_idx += 1
+ print(f"🔄 Now tracking: {self.patrol_points[self.current_target_idx]['label']}")
+ else:
+ # Complete patrol cycle
+ self.patrol_completed = True
+ self.patrol_cycles += 1
+ print(f"\n🏁 Completed patrol cycle {self.patrol_cycles}!")
+
+ # Reset for next cycle
+ time.sleep(self.target_switch_cooldown)
+ self.current_target_idx = 0
+ self.patrol_completed = False
+ self.last_target_switch_time = time.time()
+ print(f"🔄 Restarting patrol - tracking start point")
+
+ # Calculate heading error
+ torso_quat = self.data.xquat[self.torso_id]
+ robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
+ torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 + torso_quat[3] ** 2)
+ target_yaw = np.arctan2(target_vector[1], target_vector[0])
+ yaw_error = target_yaw - robot_yaw
+ yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
+
+ # Reset control commands
+ self.data.ctrl[:self.model.nu - 6] = 0.0
+
+ # -------------------------- Movement Control --------------------------
+ cycle = elapsed_time % self.gait_period
+ phase = cycle / self.gait_period
+
+ if self.return_to_path:
+ # Return to path mode
+ return_phase = (elapsed_time - self.return_to_path_start) / self.return_to_path_duration
+ return_speed = 1.5 * np.cos(return_phase * np.pi)
+
+ # Heading control
+ abdomen_z_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
+ hip_z_right_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_right")
+ hip_z_left_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_left")
+
+ if 0 <= abdomen_z_id < self.model.nu - 6:
+ self.data.ctrl[abdomen_z_id] = self.heading_kp * yaw_error * return_speed
+ if 0 <= hip_z_right_id < self.model.nu - 6:
+ self.data.ctrl[hip_z_right_id] = -yaw_error * return_speed * 0.8
+ if 0 <= hip_z_left_id < self.model.nu - 6:
+ self.data.ctrl[hip_z_left_id] = yaw_error * return_speed * 0.8
+
+ # Stabilize stance
+ for side in ["right", "left"]:
+ hip_y_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
+ knee_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
+ ankle_y_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_y_{side}")
+
+ if 0 <= hip_y_id < self.model.nu - 6:
+ self.data.ctrl[hip_y_id] = -0.8
+ if 0 <= knee_id < self.model.nu - 6:
+ self.data.ctrl[knee_id] = 1.1
+ if 0 <= ankle_y_id < self.model.nu - 6:
+ self.data.ctrl[ankle_y_id] = 0.4
+
+ elif self.avoid_obstacle:
+ # Obstacle avoidance mode
+ avoid_phase = (elapsed_time - self.obstacle_avoidance_start) / self.obstacle_avoidance_duration
+ turn_speed = 1.6 * np.sin(avoid_phase * np.pi)
+
+ # Turn control
+ hip_z_right_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_right")
+ hip_z_left_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_left")
+ abdomen_z_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
+
+ if 0 <= hip_z_right_id < self.model.nu - 6:
+ self.data.ctrl[hip_z_right_id] = self.turn_direction * turn_speed * 1.2
+ if 0 <= hip_z_left_id < self.model.nu - 6:
+ self.data.ctrl[hip_z_left_id] = -self.turn_direction * turn_speed * 1.2
+ if 0 <= abdomen_z_id < self.model.nu - 6:
+ self.data.ctrl[abdomen_z_id] = self.turn_direction * turn_speed * 2.0
+
+ # Enhanced balance
+ for side in ["right", "left"]:
+ hip_y_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
+ knee_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
+ ankle_x_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_x_{side}")
+
+ if 0 <= hip_y_id < self.model.nu - 6:
+ self.data.ctrl[hip_y_id] = -1.1
+ if 0 <= knee_id < self.model.nu - 6:
+ self.data.ctrl[knee_id] = 1.3
+ if 0 <= ankle_x_id < self.model.nu - 6:
+ self.data.ctrl[ankle_x_id] = 0.3
+
+ else:
+ # Normal patrol mode
+ # Heading control
+ abdomen_z_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
+ if 0 <= abdomen_z_id < self.model.nu - 6:
+ self.data.ctrl[abdomen_z_id] = self.heading_kp * yaw_error * 0.12
+
+ # Leg gait control
+ for side, sign in [("right", 1), ("left", -1)]:
+ swing_phase = (phase + 0.5 * sign) % 1.0
+
+ # Joint IDs
+ hip_x_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_x_{side}")
+ hip_z_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_z_{side}")
+ hip_y_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
+ knee_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
+ ankle_y_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_y_{side}")
+ ankle_x_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_x_{side}")
+
+ # Gait commands
+ if 0 <= hip_x_id < self.model.nu - 6:
+ self.data.ctrl[hip_x_id] = self.swing_gain * np.sin(2 * np.pi * swing_phase) * self.forward_speed
+ if 0 <= hip_z_id < self.model.nu - 6:
+ self.data.ctrl[hip_z_id] = self.stance_gain * np.cos(
+ 2 * np.pi * swing_phase) * 0.2 + yaw_error * 0.12
+ if 0 <= hip_y_id < self.model.nu - 6:
+ self.data.ctrl[hip_y_id] = -0.95 * np.sin(2 * np.pi * swing_phase) - 0.45
+ if 0 <= knee_id < self.model.nu - 6:
+ self.data.ctrl[knee_id] = 1.25 * np.sin(2 * np.pi * swing_phase) + 0.75
+ if 0 <= ankle_y_id < self.model.nu - 6:
+ self.data.ctrl[ankle_y_id] = 0.35 * np.cos(2 * np.pi * swing_phase)
+ if 0 <= ankle_x_id < self.model.nu - 6:
+ self.data.ctrl[ankle_x_id] = 0.18 * np.sin(2 * np.pi * swing_phase)
+
+ # Torso balance control
+ abdomen_x_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_x")
+ abdomen_y_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_y")
+
+ pitch = 2 * (torso_quat[1] * torso_quat[3] - torso_quat[0] * torso_quat[2])
+ roll = 2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3])
+
+ if 0 <= abdomen_x_id < self.model.nu - 6:
+ self.data.ctrl[abdomen_x_id] = self.balance_kp * (self.torso_roll_target - roll) - self.balance_kd * \
+ self.data.qvel[abdomen_x_id]
+ if 0 <= abdomen_y_id < self.model.nu - 6:
+ self.data.ctrl[abdomen_y_id] = self.balance_kp * (self.torso_pitch_target - pitch) - self.balance_kd * \
+ self.data.qvel[abdomen_y_id]
+
+ # Arm swing for balance
+ for side, sign in [("right", 1), ("left", -1)]:
+ shoulder1_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"shoulder1_{side}")
+ shoulder2_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"shoulder2_{side}")
+ elbow_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"elbow_{side}")
+
+ shoulder1_cmd = 0.22 * np.sin(2 * np.pi * (phase + 0.5 * sign))
+ shoulder2_cmd = 0.17 * np.cos(2 * np.pi * (phase + 0.5 * sign))
+ elbow_cmd = -0.42 * np.sin(2 * np.pi * (phase + 0.5 * sign)) - 0.27
+
+ if 0 <= shoulder1_id < self.model.nu - 6:
+ self.data.ctrl[shoulder1_id] = shoulder1_cmd
+ if 0 <= shoulder2_id < self.model.nu - 6:
+ self.data.ctrl[shoulder2_id] = shoulder2_cmd
+ if 0 <= elbow_id < self.model.nu - 6:
+ self.data.ctrl[elbow_id] = elbow_cmd
+
+ def _print_status(self, elapsed_time):
+ """Print robot status (reduced frequency to avoid clutter)"""
+ if (elapsed_time - self.last_print_time) < 2.0 or self.torso_id == -1:
+ return
+
+ self.last_print_time = elapsed_time
+ current_target = self.patrol_points[self.current_target_idx]
+ torso_pos = self.data.xpos[self.torso_id]
+ distance_to_target = np.linalg.norm(current_target["pos"] - torso_pos[:2])
+
+ # Status text
+ if self.patrol_completed:
+ status = f"Patrol completed! Cycles: {self.patrol_cycles}"
+ nav_info = "Waiting to restart"
+ elif self.avoid_obstacle:
+ status = f"Avoiding obstacle (Turn {self.turn_dir_label})"
+ nav_info = f"Target: {current_target['label']} (Distance: {distance_to_target:.2f}m)"
+ elif self.return_to_path:
+ status = "Returning to patrol path"
+ nav_info = f"Target: {current_target['label']} (Distance: {distance_to_target:.2f}m)"
+ else:
+ status = f"Tracking {current_target['label']}"
+ nav_info = f"Progress: {self.current_target_idx + 1}/{len(self.patrol_points)} | Distance: {distance_to_target:.2f}m"
+
+ # Obstacle info
+ obstacle_info = f"{self.closest_wall_info['name']}: {self.closest_wall_info['distance']:.2f}m" if \
+ self.closest_wall_info["name"] else "None"
+
+ # Print status line
+ print(
+ f"\r🕒 {elapsed_time:.1f}s | 📍 x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | 🗺️ {nav_info} | 🛡️ {obstacle_info} | 📊 {status}",
+ end=""
+ )
+
+ def run_simulation(self):
+ """Main simulation loop"""
+ print("🤖 DeepMind Humanoid Simulation Started")
+ print("📌 Features: Dynamic Obstacle Avoidance + Moving Target Tracking")
+ print("🔍 Press Ctrl+C to stop simulation\n")
+
+ with viewer.launch_passive(self.model, self.data) as viewer_instance:
+ self.sim_start_time = time.time()
+
+ try:
+ while viewer_instance.is_running():
+ elapsed_time = time.time() - self.sim_start_time
+
+ # Core control sequence
+ self._control_dynamic_obstacles(elapsed_time) # Step 1: Move obstacles
+ self._update_dynamic_targets(elapsed_time) # Step 2: Update targets
+ self._detect_obstacles(elapsed_time) # Step 3: Detect obstacles
+ self._control_robot_gait(elapsed_time) # Step 4: Control robot
+ self._print_status(elapsed_time) # Step 5: Print status
+
+ # Step simulation
+ mujoco.mj_step(self.model, self.data)
+ viewer_instance.sync()
+ time.sleep(self.model.opt.timestep * 2)
+
+ except KeyboardInterrupt:
+ print("\n\n🛑 Simulation interrupted by user")
+ except Exception as e:
+ print(f"\n\n❌ Simulation error: {e}")
+ import traceback
+ traceback.print_exc()
+ finally:
+ # Final statistics
+ if self.torso_id != -1:
+ elapsed_time = time.time() - self.sim_start_time
+ torso_pos = self.data.xpos[self.torso_id]
+ print(f"\n\n📋 Simulation Summary:")
+ print(f" Total runtime: {elapsed_time:.1f} seconds")
+ print(f" Patrol cycles completed: {self.patrol_cycles}")
+ print(f" Final position: x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f}")
+ print("🤖 Simulation ended successfully")
if __name__ == "__main__":
- model_file = "Robot_move_straight.xml" # 你的模型文件名
- control_robot(model_file)
\ No newline at end of file
+ # Set default model path (relative path for GitHub compatibility)
+ model_file = "Robot_move_straight.xml"
+
+ # Check if model file exists
+ if not os.path.exists(model_file):
+ print(f"❌ Model file not found: {model_file}")
+ sys.exit(1)
+
+ # Initialize and run controller
+ controller = DynamicPatrolController(model_file)
+ controller.run_simulation()
\ No newline at end of file