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..63d55ab293 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)
- # 设置直行速度(左右轮相同速度)
- forward_speed = 1.0
- data.ctrl[0] = forward_speed # 左轮速度
- data.ctrl[1] = forward_speed # 右轮速度
+def main():
+ """
+ Main launcher function
+ """
+ # Welcome message
+ 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..3f5b1845c3 100644
--- a/src/embodied_robot/robot_walk/Robot_move_straight.xml
+++ b/src/embodied_robot/robot_walk/Robot_move_straight.xml
@@ -1,333 +1,279 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
+
+
-
-
-
-
-
-
-
+
-
-
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ 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..7ac6cf6c08 100644
--- a/src/embodied_robot/robot_walk/move_straight.py
+++ b/src/embodied_robot/robot_walk/move_straight.py
@@ -1,31 +1,73 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+DeepMind Humanoid Robot Patrol Simulation
+Multi-target patrol with dynamic obstacle avoidance
+UTF-8 encoded, no Chinese characters, compatible with GitHub
+"""
+
import mujoco
from mujoco import viewer
import time
import numpy as np
import random
from collections import deque
-
-# 设置随机种子保证可复现
+import os
+import sys
+
+# ====================== Disable all log output (cross-version compatible) ======================
+os.environ['MUJOCO_QUIET'] = '1'
+os.environ['MUJOCO_GL'] = 'egl'
+
+# Optional: Full silent mode (uncomment to disable all console output)
+# class QuietStream:
+# def write(self, msg):
+# pass
+# def flush(self):
+# pass
+# sys.stdout = QuietStream()
+# sys.stderr = QuietStream()
+
+# Set random seed for reproducibility
np.random.seed(42)
random.seed(42)
def control_robot(model_path):
"""
- 控制DeepMind Humanoid模型:复杂动态环境下的目标导航
- 特性:多动态障碍(正弦组合/随机游走/圆周运动)+ 多障碍优先级避障 + 目标导航
+ Control DeepMind Humanoid model: Multi-target patrol navigation in dynamic environments
+ Features:
+ 1. Dynamic obstacles (sinusoidal/random walk/circular motion) + priority-based avoidance
+ 2. 5 fixed patrol points, sequential navigation with loop
+ 3. No log files, minimal console output
"""
- # 加载模型和数据
+ # Load model and data
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轴)
+ # -------------------------- Multi-target patrol configuration --------------------------
+ PATROL_POINTS = [
+ {"name": "patrol_target_1", "pos": np.array([0.0, 0.0]), "label": "Start Point"},
+ {"name": "patrol_target_2", "pos": np.array([4.0, -2.0]), "label": "Patrol Point 1 (SW)"},
+ {"name": "patrol_target_3", "pos": np.array([8.0, 2.0]), "label": "Patrol Point 2 (NE)"},
+ {"name": "patrol_target_4", "pos": np.array([10.0, -1.0]), "label": "Patrol Point 3 (NW)"},
+ {"name": "patrol_target_5", "pos": np.array([12.0, 0.0]), "label": "Final Point"}
+ ]
+ target_reached_threshold = 0.8
+ current_target_index = 0
+ patrol_cycles = 0
+ patrol_completed = False
+ target_switch_cooldown = 2.0
+ last_target_switch_time = 0
+
+ # Initialize patrol point IDs
+ patrol_point_ids = {}
+ for point in PATROL_POINTS:
+ point_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, point["name"])
+ patrol_point_ids[point["name"]] = point_id
+
+ # -------------------------- Dynamic obstacle initialization --------------------------
+ # Obstacle 1 (wall2): Sinusoidal motion (Y+Z axis)
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")
@@ -39,7 +81,7 @@ def control_robot(model_path):
"z_amp": 0.4, "z_freq": 0.3, "z_phase": random.uniform(0, 2 * np.pi)
}
- # 障碍2(wall3):随机游走运动(X+Y轴)
+ # Obstacle 2 (wall3): Random walk (X+Y axis)
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")
@@ -56,7 +98,7 @@ def control_robot(model_path):
}
wall3_last_switch = {"x": 0.0, "y": 0.0}
- # 障碍3(wall4):圆周运动(旋转+径向)
+ # Obstacle 3 (wall4): Circular motion (rotation + radial)
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")
@@ -71,28 +113,17 @@ def control_robot(model_path):
"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等基座
+ # -------------------------- Obstacle detection initialization --------------------------
valid_wall_names = ["wall1", "wall2", "wall3", "wall4"]
wall_ids = []
wall_names = []
- wall_types = {} # 标记障碍类型:fixed/dynamic1/dynamic2/dynamic3
+ wall_types = {}
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":
@@ -103,46 +134,43 @@ def control_robot(model_path):
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]}")
- # -------------------------- 核心参数配置 --------------------------
- # 多障碍避障参数
+ # -------------------------- Core parameters --------------------------
+ # Obstacle avoidance parameters
avoid_obstacle = False
- obstacle_distance_threshold = 2.0 # 增大避障触发距离
+ obstacle_distance_threshold = 2.0
obstacle_avoidance_time = 0
- obstacle_avoidance_duration = 5.0 # 延长避障时间
- turn_direction = 0
+ obstacle_avoidance_duration = 5.0
+ turn_direction = 0 # -1 = left, 1 = right
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
- # 障碍位置缓存(用于运动预判)
+ closest_wall_name = ""
+ turn_dir_label = "" # "Left" / "Right"
wall_pos_history = {name: deque(maxlen=10) for name in wall_names}
- # 步态控制参数(降低速度,增强稳定性)
+ # Gait control parameters
gait_period = 2.2
swing_gain = 0.8
stance_gain = 0.75
forward_speed = 0.3
- heading_kp = 90.0 # 增大朝向增益
+ heading_kp = 90.0
- # 姿态稳定参数
+ # Balance control parameters
torso_pitch_target = 0.0
torso_roll_target = 0.0
balance_kp = 110.0
balance_kd = 18.0
- # 启动可视化器
- mujoco.set_mjcb_control(None)
+ # Launch viewer
with viewer.launch_passive(model, data) as viewer_instance:
- print("\nDeepMind Humanoid复杂动态环境导航仿真启动!")
- print(f"导航逻辑:向目标点({TARGET_POS[0]},{TARGET_POS[1]})移动 → 多障碍检测 → 优先级避障 → 回归路径 → 到达目标")
+ print("🤖 Robot multi-target patrol simulation started (no log mode)")
+
start_time = time.time()
+ last_print_time = 0
try:
while True:
@@ -150,23 +178,20 @@ def control_robot(model_path):
break
elapsed_time = time.time() - start_time
+ current_target = PATROL_POINTS[current_target_index]
- # -------------------------- 1. 多动态障碍运动控制 --------------------------
- # 障碍1(wall2):Y+Z轴正弦组合运动
+ # -------------------------- 1. Dynamic obstacle control --------------------------
+ # Obstacle 1 (wall2): Sinusoidal motion
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):随机游走运动
+ # Obstacle 2 (wall3): Random walk
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)
@@ -176,47 +201,61 @@ def control_robot(model_path):
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):圆周运动
+ # Obstacle 3 (wall4): Circular motion
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:
+ # -------------------------- 2. Multi-target navigation logic --------------------------
+ yaw_error = 0.0
+ distance_to_target = float('inf')
+
+ if torso_id != -1 and not stop_walk:
torso_pos = data.xpos[torso_id]
robot_xy = torso_pos[:2]
- target_vector = TARGET_POS - robot_xy
+
+ # Calculate distance to current target
+ target_vector = current_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
+ # Check if target is reached
+ if (distance_to_target < target_reached_threshold and
+ not patrol_completed and
+ elapsed_time - last_target_switch_time > target_switch_cooldown):
- # 计算机器人朝向和目标方向
+ print(f"\n✅ Reached: {current_target['label']} (x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f})")
+ last_target_switch_time = elapsed_time
+
+ # Switch to next target
+ if current_target_index < len(PATROL_POINTS) - 1:
+ current_target_index += 1
+ print(f"🔄 Navigating to: {PATROL_POINTS[current_target_index]['label']}")
+ else:
+ # Complete one patrol cycle
+ patrol_completed = True
+ patrol_cycles += 1
+ print(f"\n🏁 Completed patrol cycle {patrol_cycles}!")
+
+ # Reset for next cycle
+ time.sleep(target_switch_cooldown)
+ current_target_index = 0
+ patrol_completed = False
+ last_target_switch_time = time.time()
+ print(f"🔄 Restarting patrol")
+
+ # Calculate heading error
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[
@@ -225,13 +264,12 @@ def control_robot(model_path):
yaw_error = target_yaw - robot_yaw
yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
- # -------------------------- 3. 多障碍检测与优先级排序 --------------------------
+ # -------------------------- 3. Obstacle detection & priority sorting --------------------------
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:
+ if wall_ids and torso_id != -1 and not stop_walk and not patrol_completed:
torso_pos = data.xpos[torso_id]
wall_distances = []
@@ -239,26 +277,19 @@ def control_robot(model_path):
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])
- # 动态障碍位置预判(基于历史数据)
+ # Predict dynamic obstacle position
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
-
+ weighted_distance = predicted_distance / wall_priority[wall_type]
wall_distances.append({
"name": wall_name,
"type": wall_type,
@@ -269,9 +300,7 @@ def control_robot(model_path):
"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"]
@@ -280,66 +309,61 @@ def control_robot(model_path):
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:
- # 触发避障
+ # -------------------------- 4. Obstacle avoidance state control --------------------------
+ if closest_wall_id != -1 and torso_id != -1 and not stop_walk and not patrol_completed:
+ # Trigger obstacle avoidance
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]
- # 叉乘判断转向方向(优先绕开障碍且朝向目标)
+ target_relative = current_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 "右转"
+ turn_dir_label = "Left" if turn_direction == -1 else "Right"
- print(f"\n⚠️ 检测到高优先级障碍【{closest_wall_name}({closest_wall_type})】!")
- print(f" 预测距离:{distance_to_closest_wall:.2f}m | 转向方向:{dir_name}")
+ print(
+ f"\n⚠️ Obstacle avoidance: {closest_wall_name} (distance: {distance_to_closest_wall:.2f}m) Turn {turn_dir_label}")
- # 避障完成,回归导航路径
+ # Complete avoidance, return to path
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"✅ 避障完成,回归导航路径...")
+ print(f"✅ Obstacle avoidance completed, returning to path")
- # 回归完成,继续向目标点移动
+ # Return completed
if return_to_path and (time.time() - return_time) > return_duration:
return_to_path = False
- print(f"✅ 回归路径完成,继续向目标点移动...")
+ print(f"✅ Path return completed")
- # -------------------------- 5. 步态周期计算 --------------------------
+ # -------------------------- 5. Gait cycle calculation --------------------------
cycle = elapsed_time % gait_period
phase = cycle / gait_period
- # -------------------------- 6. 关节控制核心逻辑 --------------------------
- data.ctrl[:model.nu - 6] = 0.0 # 重置机器人控制指令(保留障碍电机)
+ # -------------------------- 6. Joint control logic --------------------------
+ data.ctrl[:model.nu - 6] = 0.0 # Reset control commands
- if stop_walk or target_reached:
+ if stop_walk or patrol_completed:
continue
elif return_to_path:
- # 回归导航路径:精准朝向目标点
+ # Return to navigation 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_vector = current_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))
- # 转向控制
+ # Heading control
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")
@@ -351,7 +375,7 @@ def control_robot(model_path):
if 0 <= hip_z_left_act_id < model.nu - 6:
data.ctrl[hip_z_left_act_id] = yaw_error * return_speed * 0.8
- # 稳定站立
+ # Stabilize stance
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}")
@@ -364,11 +388,11 @@ def control_robot(model_path):
data.ctrl[ankle_y_act_id] = 0.4
elif avoid_obstacle:
- # 多障碍避障模式:增强转向稳定性
+ # Obstacle avoidance mode
avoid_phase = (time.time() - obstacle_avoidance_time) / obstacle_avoidance_duration
turn_speed = 1.6 * np.sin(avoid_phase * np.pi)
- # 转向控制
+ # Turn control
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")
@@ -380,7 +404,7 @@ def control_robot(model_path):
if 0 <= abdomen_z_act_id < model.nu - 6:
data.ctrl[abdomen_z_act_id] = turn_direction * turn_speed * 2.0
- # 增强平衡控制
+ # Enhanced balance control
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}")
@@ -393,27 +417,27 @@ def control_robot(model_path):
data.ctrl[ankle_x_act_id] = 0.3
else:
- # 正常导航模式:向目标点移动
+ # Normal navigation mode
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_vector = current_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))
- # 朝向目标点的转向控制
+ # Heading control
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
- # 腿部步态控制
+ # Leg gait control
for side, sign in [("right", 1), ("left", -1)]:
swing_phase = (phase + 0.5 * sign) % 1.0
- # 查询电机ID
+ # Get actuator IDs
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}")
@@ -421,7 +445,7 @@ def control_robot(model_path):
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}")
- # 腿部关节控制
+ # Joint control commands
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:
@@ -436,7 +460,7 @@ def control_robot(model_path):
if 0 <= ankle_x_act_id < model.nu - 6:
data.ctrl[ankle_x_act_id] = 0.18 * np.sin(2 * np.pi * swing_phase)
- # 躯干稳定控制
+ # Torso balance control
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")
@@ -452,7 +476,7 @@ def control_robot(model_path):
data.ctrl[abdomen_y_act_id] = balance_kp * (torso_pitch_target - pitch) - balance_kd * \
data.qvel[abdomen_y_act_id]
- # 手臂自然摆动
+ # Arm swing control
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}")
@@ -469,53 +493,61 @@ def control_robot(model_path):
if 0 <= elbow_act_id < model.nu - 6:
data.ctrl[elbow_act_id] = elbow_cmd
- # -------------------------- 7. 仿真推进 --------------------------
+ # -------------------------- 7. Simulation step --------------------------
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"
+ # -------------------------- 8. Status output (reduced frequency) --------------------------
+ if torso_id != -1 and (elapsed_time - last_print_time) > 2.0:
+ last_print_time = elapsed_time
+
+ if patrol_completed:
+ status = f"Patrol completed! Cycles: {patrol_cycles}"
+ nav_info = "Waiting to restart"
obstacle_info = "—"
elif stop_walk:
- status = "已停止"
+ status = "Stopped"
nav_info = "—"
obstacle_info = "—"
else:
if return_to_path:
- status = "回归导航路径中"
+ status = "Returning to path"
elif avoid_obstacle:
- status = f"避障中({dir_name} | {closest_wall_name})"
+ status = f"Avoiding obstacle (Turn {turn_dir_label})"
else:
- status = "向目标点移动中"
+ status = f"Moving to {current_target['label']}"
- # 导航信息
- 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}°"
+ # Navigation info
+ nav_progress = f"{current_target_index + 1}/{len(PATROL_POINTS)}"
+ nav_info = f"Remaining: {distance_to_target:.2f}m | Progress: {nav_progress}"
- # 障碍信息
+ # Obstacle info
if closest_wall_name:
- obstacle_info = f"{closest_wall_name}({closest_wall_type}):{distance_to_closest_wall:.2f}m"
+ obstacle_info = f"{closest_wall_name}: {distance_to_closest_wall:.2f}m"
else:
- obstacle_info = "无"
+ obstacle_info = "None"
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}",
+ 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仿真被用户中断")
+ print("\n\n🛑 Simulation interrupted by user")
except Exception as e:
- print(f"\n\n运行错误:{e}")
+ print(f"\n\n❌ Runtime error: {e}")
import traceback
traceback.print_exc()
+ finally:
+ # Final statistics
+ if torso_id != -1:
+ print(f"\n\n📋 Simulation ended:")
+ print(f" Total time: {elapsed_time:.1f}s | Cycles completed: {patrol_cycles}")
+ print(f" Final position: x={data.xpos[torso_id][0]:.2f}, y={data.xpos[torso_id][1]:.2f}")
if __name__ == "__main__":
- model_file = "Robot_move_straight.xml" # 你的模型文件名
+ model_file = "Robot_move_straight.xml"
control_robot(model_file)
\ No newline at end of file