diff --git a/src/simple_wheeled_vehicle/3.py b/src/simple_wheeled_vehicle/3.py
index 11ce5fde0d..1825df88c6 100644
--- a/src/simple_wheeled_vehicle/3.py
+++ b/src/simple_wheeled_vehicle/3.py
@@ -1,59 +1,188 @@
"""
-自动巡航小车
-- 恒定速度 0.0005 m/s
-- 转向回正(直走)
-- R 键仍可复位
+自动巡航小车 - 带前方障碍物检测与避障
+- 提高速度至0.00075 m/s(提高50%)
+- 前方0.5米内有障碍物时停止
+- 持续扫描前方障碍物
+- R 键可复位
"""
import mujoco
import mujoco.viewer
import numpy as np
from pynput import keyboard
+import math
-# ---------------- 键盘 ------------------
+# ------------------- 键盘监听 -------------------
KEYS = {keyboard.KeyCode.from_char('r'): False}
def on_press(k):
if k in KEYS: KEYS[k] = True
+
def on_release(k):
if k in KEYS: KEYS[k] = False
+
keyboard.Listener(on_press=on_press, on_release=on_release).start()
-# ---------------- 加载模型 --------------
+# ------------------- 加载模型 -------------------
model = mujoco.MjModel.from_xml_path("wheeled_car.xml")
data = mujoco.MjData(model)
-# ---------------- 参数 ------------------
-CRUISE_SPEED = 0.0005 # ← 龟速恒定前进
-steer_target = 0.0 # 直走
+# ------------------- 参数设置 -------------------
+# 提高50%的速度:0.0005 * 1.5 = 0.00075
+CRUISE_SPEED = 0.00075 # 提高50%的巡航速度
+OBSTACLE_DISTANCE_THRESHOLD = 0.5 # 障碍物检测距离改为0.5米
+SAFE_DISTANCE = 0.2 # 安全距离,小于此距离时紧急停止(也相应减小)
+
+# 障碍物名称列表(根据模型文件中的障碍物定义)
+OBSTACLE_NAMES = [
+ 'obs_box1', 'obs_box2', 'obs_box3', 'obs_box4',
+ 'obs_ball1', 'obs_ball2', 'obs_ball3',
+ 'wall1', 'wall2', 'front_dark_box'
+]
-# ---------------- 复位 ------------------
+# 提前获取所有障碍物的body id,避免每次循环都查找
+obstacle_ids = {}
+for obs_name in OBSTACLE_NAMES:
+ obs_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, obs_name)
+ if obs_id != -1:
+ obstacle_ids[obs_name] = obs_id
+
+# 小车底盘body id
+chassis_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "chassis")
+
+# ------------------- 复位函数 -------------------
def reset_car():
mujoco.mj_resetData(model, data)
- data.qpos[2] = 0.03
- print("\r>>> 已复位 <<<", end='', flush=True)
+ data.qpos[2] = 0.03 # 设置初始高度
+ print("\n>>> 已复位 <<<")
+
+# ------------------- 障碍物检测函数(简化版) -------------------
+def check_front_obstacle():
+ """检测小车前方是否有障碍物(简化版本)"""
+
+ # 获取小车当前位置
+ chassis_pos = data.body(chassis_body_id).xpos # 位置 (x, y, z)
+
+ # 扫描所有障碍物
+ min_distance = float('inf')
+ closest_obstacle = None
+
+ for obs_name, obs_id in obstacle_ids.items():
+ # 获取障碍物位置
+ obs_pos = data.body(obs_id).xpos
+
+ # 计算相对于小车的位置
+ dx = obs_pos[0] - chassis_pos[0] # x方向距离
+ dy = obs_pos[1] - chassis_pos[1] # y方向距离
+
+ # 计算水平距离
+ distance = math.sqrt(dx**2 + dy**2)
-# ---------------- 主循环 ----------------
+ # 只考虑小车前方的障碍物(dx > 0 表示在小车前方)
+ if dx > 0 and distance < OBSTACLE_DISTANCE_THRESHOLD:
+ # 计算障碍物相对于小车前向方向的横向偏移
+ # 假设小车前向是x轴正方向
+ lateral_distance = abs(dy)
+
+ # 如果横向距离小于小车的宽度(约0.3米),则认为在正前方
+ if lateral_distance < 0.3:
+ if distance < min_distance:
+ min_distance = distance
+ closest_obstacle = obs_name
+
+ # 返回检测结果
+ if closest_obstacle is not None:
+ if min_distance < SAFE_DISTANCE:
+ return 2, min_distance, closest_obstacle # 紧急停止
+ else:
+ return 1, min_distance, closest_obstacle # 检测到障碍物
+
+ return 0, 0, None # 无障碍物
+
+# ------------------- 主循环 -------------------
mujoco.mj_resetData(model, data)
+auto_stop = False # 自动停止标志
+stop_counter = 0 # 停止计数器(用于避免频繁启停)
+
with mujoco.viewer.launch_passive(model, data) as viewer:
viewer.cam.distance = 2.5
viewer.cam.elevation = -25
+
while viewer.is_running():
- if KEYS[keyboard.KeyCode.from_char('r')]:
+ # 检查R键复位
+ if KEYS.get(keyboard.KeyCode.from_char('r'), False):
reset_car()
+ auto_stop = False
+ stop_counter = 0
KEYS[keyboard.KeyCode.from_char('r')] = False
- # ---- 自动巡航:恒定速度 + 直走 ----
- data.ctrl[0] = steer_target # 前左转向 = 0
- data.ctrl[1] = steer_target # 前右转向 = 0
- data.ctrl[2] = CRUISE_SPEED # 前左驱动
- data.ctrl[3] = CRUISE_SPEED # 前右驱动
- data.ctrl[4] = CRUISE_SPEED # 后左驱动
- data.ctrl[5] = CRUISE_SPEED # 后右驱动
+ # 检测前方障碍物
+ obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
- # ---- 仿真 + HUD ----
+ # 控制逻辑
+ if auto_stop:
+ # 当前处于自动停止状态
+ if obstacle_status == 0:
+ # 前方无障碍物,恢复行驶
+ auto_stop = False
+ stop_counter = 0
+ else:
+ # 前方仍有障碍物,保持停止
+ stop_counter += 1
+ # 所有驱动器设为0
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+ else:
+ # 正常行驶状态
+ if obstacle_status == 2:
+ # 紧急停止(距离过近)
+ auto_stop = True
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+ stop_counter = 20 # 由于速度提高,减少停止时间
+ elif obstacle_status == 1:
+ # 检测到前方障碍物,准备停止
+ # 由于速度提高,提前触发停止(减少延迟帧数)
+ if stop_counter > 5: # 减少延迟帧数
+ auto_stop = True
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+ else:
+ stop_counter += 1
+ # 正常巡航
+ data.ctrl[0] = 0.0 # 前左转向
+ data.ctrl[1] = 0.0 # 前右转向
+ data.ctrl[2] = CRUISE_SPEED # 前左驱动
+ data.ctrl[3] = CRUISE_SPEED # 前右驱动
+ data.ctrl[4] = CRUISE_SPEED # 后左驱动
+ data.ctrl[5] = CRUISE_SPEED # 后右驱动
+ else:
+ # 无障碍物,正常巡航
+ stop_counter = 0
+ data.ctrl[0] = 0.0 # 前左转向
+ data.ctrl[1] = 0.0 # 前右转向
+ data.ctrl[2] = CRUISE_SPEED # 前左驱动
+ data.ctrl[3] = CRUISE_SPEED # 前右驱动
+ data.ctrl[4] = CRUISE_SPEED # 后左驱动
+ data.ctrl[5] = CRUISE_SPEED # 后右驱动
+
+ # 仿真步骤
mujoco.mj_step(model, data)
+
+ # 显示信息
vel = np.linalg.norm(data.qvel[:3])
- print(f"\rspeed: {vel:7.5f} m/s", end='', flush=True)
+ status_text = "前进"
+ if auto_stop:
+ status_text = "停止"
+ elif obstacle_status > 0:
+ status_text = "减速"
+
+ obstacle_info = ""
+ if obstacle_status > 0 and obstacle_name:
+ obstacle_info = f", 障碍物: {obstacle_name}({obstacle_distance:.2f}m)"
+
+ print(f"\r状态: {status_text}, 速度: {vel:7.5f} m/s{obstacle_info}", end='', flush=True)
+
+ # 同步视图
viewer.sync()
- print()
\ No newline at end of file
+print()
\ No newline at end of file
diff --git a/src/simple_wheeled_vehicle/5.py b/src/simple_wheeled_vehicle/5.py
new file mode 100644
index 0000000000..457a4b4b57
--- /dev/null
+++ b/src/simple_wheeled_vehicle/5.py
@@ -0,0 +1,283 @@
+"""
+自动巡航小车 - 带前方障碍物检测与90度转向避障
+- 提高速度至0.0015 m/s(提高两倍)
+- 前方0.5米内有障碍物时停止并进行90度转向
+- 转向后恢复巡航速度
+- R 键可复位
+"""
+import mujoco
+import mujoco.viewer
+import numpy as np
+from pynput import keyboard
+import math
+import random
+import time
+
+# ------------------- 键盘监听 -------------------
+KEYS = {keyboard.KeyCode.from_char('r'): False}
+
+def on_press(k):
+ if k in KEYS: KEYS[k] = True
+
+def on_release(k):
+ if k in KEYS: KEYS[k] = False
+
+keyboard.Listener(on_press=on_press, on_release=on_release).start()
+
+# ------------------- 加载模型 -------------------
+model = mujoco.MjModel.from_xml_path("wheeled_car.xml")
+data = mujoco.MjData(model)
+
+# ------------------- 参数设置 -------------------
+# 提高两倍的速度:0.0005 * 2 = 0.0015
+CRUISE_SPEED = 0.0015 # 提高两倍的巡航速度
+TURN_SPEED = CRUISE_SPEED * 0.5 # 转向时的速度(更低以确保安全)
+OBSTACLE_DISTANCE_THRESHOLD = 0.5 # 障碍物检测距离
+SAFE_DISTANCE = 0.2 # 安全距离
+TURN_ANGLE = math.pi / 2 # 90度转向角度(π/2弧度)
+TURN_DURATION = 150 # 90度转向需要更长时间
+
+# 障碍物名称列表
+OBSTACLE_NAMES = [
+ 'obs_box1', 'obs_box2', 'obs_box3', 'obs_box4',
+ 'obs_ball1', 'obs_ball2', 'obs_ball3',
+ 'wall1', 'wall2', 'front_dark_box'
+]
+
+# 提前获取所有障碍物的body id
+obstacle_ids = {}
+for obs_name in OBSTACLE_NAMES:
+ obs_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, obs_name)
+ if obs_id != -1:
+ obstacle_ids[obs_name] = obs_id
+
+# 小车底盘body id
+chassis_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "chassis")
+
+# ------------------- 复位函数 -------------------
+def reset_car():
+ mujoco.mj_resetData(model, data)
+ data.qpos[2] = 0.03 # 设置初始高度
+ print("\n>>> 已复位 <<<")
+
+# ------------------- 障碍物检测函数 -------------------
+def check_front_obstacle():
+ """检测小车前方是否有障碍物"""
+
+ # 获取小车当前位置
+ chassis_pos = data.body(chassis_body_id).xpos # 位置 (x, y, z)
+
+ # 扫描所有障碍物
+ min_distance = float('inf')
+ closest_obstacle = None
+
+ for obs_name, obs_id in obstacle_ids.items():
+ # 获取障碍物位置
+ obs_pos = data.body(obs_id).xpos
+
+ # 计算相对于小车的位置
+ dx = obs_pos[0] - chassis_pos[0] # x方向距离
+ dy = obs_pos[1] - chassis_pos[1] # y方向距离
+
+ # 计算水平距离
+ distance = math.sqrt(dx**2 + dy**2)
+
+ # 只考虑小车前方的障碍物(dx > 0 表示在小车前方)
+ if dx > 0 and distance < OBSTACLE_DISTANCE_THRESHOLD:
+ # 计算障碍物相对于小车前向方向的横向偏移
+ lateral_distance = abs(dy)
+
+ # 如果横向距离小于小车的宽度(约0.3米),则认为在正前方
+ if lateral_distance < 0.3:
+ if distance < min_distance:
+ min_distance = distance
+ closest_obstacle = obs_name
+
+ # 返回检测结果
+ if closest_obstacle is not None:
+ if min_distance < SAFE_DISTANCE:
+ return 2, min_distance, closest_obstacle # 紧急停止
+ else:
+ return 1, min_distance, closest_obstacle # 检测到障碍物
+
+ return 0, 0, None # 无障碍物
+
+# ------------------- 随机选择90度转向方向 -------------------
+def choose_90_degree_turn():
+ """随机选择向左或向右90度转向"""
+ if random.random() > 0.5:
+ return TURN_ANGLE # 右转90度
+ else:
+ return -TURN_ANGLE # 左转90度
+
+# ------------------- 主循环 -------------------
+mujoco.mj_resetData(model, data)
+
+# 状态变量
+CAR_STATE = "CRUISING" # 状态: CRUISING, STOPPED, TURNING, RESUME
+turn_counter = 0
+turn_angle = 0
+turn_direction = "" # 记录转向方向
+
+with mujoco.viewer.launch_passive(model, data) as viewer:
+ viewer.cam.distance = 2.5
+ viewer.cam.elevation = -25
+
+ while viewer.is_running():
+ # 检查R键复位
+ if KEYS.get(keyboard.KeyCode.from_char('r'), False):
+ reset_car()
+ CAR_STATE = "CRUISING"
+ turn_counter = 0
+ KEYS[keyboard.KeyCode.from_char('r')] = False
+
+ # 根据当前状态执行不同操作
+ if CAR_STATE == "CRUISING":
+ # 巡航状态:检测前方障碍物
+ obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+
+ if obstacle_status == 2:
+ # 紧急停止(距离过近)
+ CAR_STATE = "STOPPED"
+ print(f"\n紧急停止!障碍物距离: {obstacle_distance:.2f}m")
+ turn_counter = 0
+
+ # 停止所有驱动
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+
+ elif obstacle_status == 1:
+ # 检测到前方障碍物,减速准备停止
+ CAR_STATE = "STOPPED"
+ print(f"\n检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),正在停止...")
+ turn_counter = 0
+
+ # 减速停止
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+ else:
+ # 无障碍物,正常巡航(两倍速度)
+ data.ctrl[0] = 0.0 # 前左转向
+ data.ctrl[1] = 0.0 # 前右转向
+ data.ctrl[2] = CRUISE_SPEED # 前左驱动
+ data.ctrl[3] = CRUISE_SPEED # 前右驱动
+ data.ctrl[4] = CRUISE_SPEED # 后左驱动
+ data.ctrl[5] = CRUISE_SPEED # 后右驱动
+
+ elif CAR_STATE == "STOPPED":
+ # 已停止状态:选择转向方向
+ turn_counter += 1
+
+ # 停止所有驱动
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+
+ # 等待几帧确保完全停止
+ if turn_counter > 10:
+ print("选择转向方向...")
+
+ # 随机选择向左或向右90度转向
+ turn_angle = choose_90_degree_turn()
+
+ # 记录转向方向
+ if turn_angle > 0:
+ turn_direction = "右转90度"
+ else:
+ turn_direction = "左转90度"
+
+ print(f"将执行{turn_direction}")
+
+ # 进入转向状态
+ CAR_STATE = "TURNING"
+ turn_counter = 0
+
+ elif CAR_STATE == "TURNING":
+ # 转向状态:执行90度转向
+ turn_counter += 1
+
+ # 设置90度转向角度
+ data.ctrl[0] = turn_angle # 前左转向
+ data.ctrl[1] = turn_angle # 前右转向
+
+ # 低速前进,帮助转向
+ data.ctrl[2] = TURN_SPEED # 前左驱动
+ data.ctrl[3] = TURN_SPEED # 前右驱动
+ data.ctrl[4] = TURN_SPEED # 后左驱动
+ data.ctrl[5] = TURN_SPEED # 后右驱动
+
+ # 显示转向信息
+ if turn_counter % 30 == 0:
+ print(f"正在执行{turn_direction},进度: {turn_counter}/{TURN_DURATION}")
+
+ # 检查转向是否完成
+ if turn_counter > TURN_DURATION:
+ print(f"{turn_direction}完成,准备恢复巡航")
+ CAR_STATE = "RESUME"
+ turn_counter = 0
+
+ elif CAR_STATE == "RESUME":
+ # 恢复状态:转向后恢复直行并恢复巡航速度
+ turn_counter += 1
+
+ # 逐渐回正转向
+ if turn_counter < 20:
+ # 逐渐减小转向角度
+ decay_factor = 1.0 - (turn_counter / 20.0)
+ current_angle = turn_angle * decay_factor
+ data.ctrl[0] = current_angle
+ data.ctrl[1] = current_angle
+
+ # 逐渐加速到巡航速度
+ speed_factor = turn_counter / 20.0
+ current_speed = TURN_SPEED + (CRUISE_SPEED - TURN_SPEED) * speed_factor
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+ else:
+ # 转向已回正,恢复两倍巡航速度
+ data.ctrl[0] = 0.0
+ data.ctrl[1] = 0.0
+ data.ctrl[2] = CRUISE_SPEED
+ data.ctrl[3] = CRUISE_SPEED
+ data.ctrl[4] = CRUISE_SPEED
+ data.ctrl[5] = CRUISE_SPEED
+
+ # 检查前方是否安全
+ obstacle_status, obstacle_distance, _ = check_front_obstacle()
+ if obstacle_status == 0:
+ print("前方安全,恢复巡航")
+ CAR_STATE = "CRUISING"
+ turn_counter = 0
+ else:
+ # 前方仍有障碍物,重新停止
+ print("转向后前方仍有障碍物,重新处理")
+ CAR_STATE = "STOPPED"
+ turn_counter = 0
+
+ # 仿真步骤
+ mujoco.mj_step(model, data)
+
+ # 显示信息
+ vel = np.linalg.norm(data.qvel[:3])
+
+ # 获取当前转向角度
+ current_steer = (data.ctrl[0] + data.ctrl[1]) / 2
+
+ # 显示状态信息
+ status_info = f"状态: {CAR_STATE}, 速度: {vel:7.5f} m/s"
+ if abs(current_steer) > 0.01:
+ status_info += f", 转向: {math.degrees(current_steer):.1f}度"
+
+ # 如果正在巡航,显示障碍物检测信息
+ if CAR_STATE == "CRUISING":
+ obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+ if obstacle_status > 0 and obstacle_name:
+ status_info += f", 前方障碍: {obstacle_name}({obstacle_distance:.2f}m)"
+
+ print(f"\r{status_info}", end='', flush=True)
+
+ # 同步视图
+ viewer.sync()
+
+print("\n程序结束")
\ No newline at end of file
diff --git a/src/simple_wheeled_vehicle/cruise2.py b/src/simple_wheeled_vehicle/cruise2.py
new file mode 100644
index 0000000000..11ce5fde0d
--- /dev/null
+++ b/src/simple_wheeled_vehicle/cruise2.py
@@ -0,0 +1,59 @@
+"""
+自动巡航小车
+- 恒定速度 0.0005 m/s
+- 转向回正(直走)
+- R 键仍可复位
+"""
+import mujoco
+import mujoco.viewer
+import numpy as np
+from pynput import keyboard
+
+# ---------------- 键盘 ------------------
+KEYS = {keyboard.KeyCode.from_char('r'): False}
+
+def on_press(k):
+ if k in KEYS: KEYS[k] = True
+def on_release(k):
+ if k in KEYS: KEYS[k] = False
+keyboard.Listener(on_press=on_press, on_release=on_release).start()
+
+# ---------------- 加载模型 --------------
+model = mujoco.MjModel.from_xml_path("wheeled_car.xml")
+data = mujoco.MjData(model)
+
+# ---------------- 参数 ------------------
+CRUISE_SPEED = 0.0005 # ← 龟速恒定前进
+steer_target = 0.0 # 直走
+
+# ---------------- 复位 ------------------
+def reset_car():
+ mujoco.mj_resetData(model, data)
+ data.qpos[2] = 0.03
+ print("\r>>> 已复位 <<<", end='', flush=True)
+
+# ---------------- 主循环 ----------------
+mujoco.mj_resetData(model, data)
+with mujoco.viewer.launch_passive(model, data) as viewer:
+ viewer.cam.distance = 2.5
+ viewer.cam.elevation = -25
+ while viewer.is_running():
+ if KEYS[keyboard.KeyCode.from_char('r')]:
+ reset_car()
+ KEYS[keyboard.KeyCode.from_char('r')] = False
+
+ # ---- 自动巡航:恒定速度 + 直走 ----
+ data.ctrl[0] = steer_target # 前左转向 = 0
+ data.ctrl[1] = steer_target # 前右转向 = 0
+ data.ctrl[2] = CRUISE_SPEED # 前左驱动
+ data.ctrl[3] = CRUISE_SPEED # 前右驱动
+ data.ctrl[4] = CRUISE_SPEED # 后左驱动
+ data.ctrl[5] = CRUISE_SPEED # 后右驱动
+
+ # ---- 仿真 + HUD ----
+ mujoco.mj_step(model, data)
+ vel = np.linalg.norm(data.qvel[:3])
+ print(f"\rspeed: {vel:7.5f} m/s", end='', flush=True)
+ viewer.sync()
+
+ print()
\ No newline at end of file
diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 90ddf5b11f..7e233c71e1 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -1,4 +1,4 @@
-