Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 11 additions & 3 deletions src/embodied_robot/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ def check_dependencies():
sys.exit(1)


def run_robot_simulation(python_exe, robot_walk_dir, script_file):
def run_robot_simulation(python_exe, robot_walk_dir, script_file,extra_args):
"""
Launch robot simulation script (run in robot_walk directory for correct path resolution)
"""
Expand All @@ -140,9 +140,12 @@ def run_robot_simulation(python_exe, robot_walk_dir, script_file):
env['MUJOCO_QUIET'] = '1'
env['PYTHONPATH'] = str(Path(__file__).resolve().parent) + os.pathsep + env.get('PYTHONPATH', '')

# 核心修改点:将 extra_args 拼接到启动命令列表中
cmd = [python_exe, str(script_file)] + extra_args

# Run script in robot_walk directory
result = subprocess.run(
[python_exe, str(script_file)],
cmd, # 使用拼接好的命令
cwd=str(robot_walk_dir),
env=env,
stdout=sys.stdout,
Expand Down Expand Up @@ -191,8 +194,13 @@ def main():
print("\n🔍 Checking dependencies...")
check_dependencies()

# 核心修改点:获取 sys.argv 中除了脚本名(main.py)之外的所有参数
extra_args = sys.argv[1:]
if extra_args:
print(f"📥 Received passthrough arguments: {' '.join(extra_args)}")

# 4. Run simulation
exit_code = run_robot_simulation(python_exe, robot_walk_dir, script_file)
exit_code = run_robot_simulation(python_exe, robot_walk_dir, script_file,extra_args)

# 5. Exit
sys.exit(exit_code)
Expand Down
35 changes: 22 additions & 13 deletions src/embodied_robot/robot_walk/move_straight.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@


class StablePatrolController:
def __init__(self, model_path):
def __init__(self, model_path,args=None):
"""Initialize robot controller with enhanced balance control and distance-priority target selection"""
# Load MuJoCo model and data
self.model = mujoco.MjModel.from_xml_path(model_path)
Expand Down Expand Up @@ -63,7 +63,7 @@ def __init__(self, model_path):
self.patrol_body_ids = {}
self.last_target_update = {i: 0.0 for i in range(len(self.patrol_points))}
self.target_movement_range = {"x": [-1.0, 9.0], "y": [-3.0, 3.0]}
self.target_move_speed = 0.2 # Slow target movement for stability
self.target_move_speed = args.target_move_speed if args else 0.2 # 动态目标移动速度 # Slow target movement for stability

# -------------------------- Obstacle Avoidance Parameters --------------------------
self.valid_wall_names = ["wall1", "wall2", "wall3", "wall4"]
Expand All @@ -89,7 +89,12 @@ def __init__(self, model_path):
self.gait_period = 3.0 # Longer gait period for stability
self.swing_gain = 0.3
self.stance_gain = 0.4
self.forward_speed = 0.05 # Reduced speed to prevent flying (fix disappearing issue)
self.forward_speed = args.forward_speed if args else 0.05 # 机器人前进速度 # Reduced speed to prevent flying (fix disappearing issue)
print("\n" + "✨" * 20)
print("📥 【系统确认】底层已成功接收到外部参数:")
print(f" 🏃 机器人设定的前进速度: {self.forward_speed}")
print(f" 🎯 巡逻目标点的移动速度: {self.target_move_speed}")
print("✨" * 20 + "\n")
self.heading_kp = 40.0
self.balance_kp = 60.0
self.balance_kd = 30.0
Expand Down Expand Up @@ -878,22 +883,26 @@ def run_simulation(self):


if __name__ == "__main__":
# Default model path
model_file = "Robot_move_straight.xml"
import argparse

# Override model path with command line argument
if len(sys.argv) > 1:
model_file = sys.argv[1]
# 设置参数解析器
parser = argparse.ArgumentParser(description="DeepMind Humanoid Patrol Controller")
parser.add_argument("--model_file", type=str, default="Robot_move_straight.xml", help="Path to MuJoCo XML model")
parser.add_argument("--forward_speed", type=float, default=0.05, help="Robot forward walking speed")
parser.add_argument("--target_move_speed", type=float, default=0.2, help="Dynamic target movement speed")

# Check if model file exists
if not os.path.exists(model_file):
print(f"❌ Model file not found: {model_file}")
# 解析参数
args = parser.parse_args()

# 检查模型文件是否存在
if not os.path.exists(args.model_file):
print(f"❌ Model file not found: {args.model_file}")
print(f"ℹ️ Current working directory: {os.getcwd()}")
sys.exit(1)

# Run simulation
# 运行仿真 (把解析好的 args 传给控制器)
try:
controller = StablePatrolController(model_file)
controller = StablePatrolController(args.model_file, args)
controller.run_simulation()
except Exception as e:
print(f"\n❌ Failed to start simulation: {e}")
Expand Down