From 6a6fa8511a27bceca29186984ffdaade28152bde Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Sun, 21 Dec 2025 15:09:32 +0800
Subject: [PATCH 1/9] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E6=B7=BB?=
=?UTF-8?q?=E5=8A=A0=E8=87=AA=E5=8A=A8=E7=BC=93=E6=85=A2=E5=B7=A1=E8=88=AA?=
=?UTF-8?q?=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/2.py | 1 -
src/simple_wheeled_vehicle/wheeled_car.xml | 1 -
2 files changed, 2 deletions(-)
diff --git a/src/simple_wheeled_vehicle/2.py b/src/simple_wheeled_vehicle/2.py
index 72e5a0d308..11ce5fde0d 100644
--- a/src/simple_wheeled_vehicle/2.py
+++ b/src/simple_wheeled_vehicle/2.py
@@ -16,7 +16,6 @@ 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()
# ---------------- 加载模型 --------------
diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 3e91223e18..e80693af65 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -8,7 +8,6 @@
-
Date: Sun, 21 Dec 2025 20:46:52 +0800
Subject: [PATCH 2/9] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E5=91=A8?=
=?UTF-8?q?=E5=9B=B4=E6=B7=BB=E5=8A=A0=E9=9A=9C=E7=A2=8D=E7=89=A9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/3.py | 59 ++++++++++++++++++++++
src/simple_wheeled_vehicle/wheeled_car.xml | 50 +++++++++++++++---
2 files changed, 103 insertions(+), 6 deletions(-)
create mode 100644 src/simple_wheeled_vehicle/3.py
diff --git a/src/simple_wheeled_vehicle/3.py b/src/simple_wheeled_vehicle/3.py
new file mode 100644
index 0000000000..11ce5fde0d
--- /dev/null
+++ b/src/simple_wheeled_vehicle/3.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 e80693af65..90ddf5b11f 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -1,4 +1,4 @@
-
+
@@ -7,22 +7,25 @@
+
+
+
+
-
-
-
+
@@ -32,6 +35,41 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -39,7 +77,7 @@
-
+
@@ -52,7 +90,7 @@
-
+
From 2a71069532229aaf6ecb7231621a6284103382be Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 02:03:59 +0800
Subject: [PATCH 3/9] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E6=96=B0?=
=?UTF-8?q?=E5=A2=9E=E7=BC=93=E6=85=A2=E8=87=AA=E5=8A=A8=E5=B7=A1=E8=88=AA?=
=?UTF-8?q?=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/3.py | 6 ------
src/simple_wheeled_vehicle/wheeled_car.xml | 24 +++++++++++-----------
2 files changed, 12 insertions(+), 18 deletions(-)
diff --git a/src/simple_wheeled_vehicle/3.py b/src/simple_wheeled_vehicle/3.py
index 11ce5fde0d..e439deac00 100644
--- a/src/simple_wheeled_vehicle/3.py
+++ b/src/simple_wheeled_vehicle/3.py
@@ -1,9 +1,3 @@
-"""
-自动巡航小车
-- 恒定速度 0.0005 m/s
-- 转向回正(直走)
-- R 键仍可复位
-"""
import mujoco
import mujoco.viewer
import numpy as np
diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 90ddf5b11f..a0f375ecf0 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -1,4 +1,4 @@
-
+
@@ -7,7 +7,6 @@
-
@@ -25,7 +24,7 @@
-
+
@@ -35,8 +34,7 @@
-
-
+
@@ -49,8 +47,6 @@
-
-
@@ -60,8 +56,6 @@
-
-
@@ -69,7 +63,11 @@
-
+
+
+
+
+
@@ -119,8 +117,10 @@
-
-
+
+
+
+
From d6d6d44e9da8127e2df296b7e02ea032ed09c1ad Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 02:19:33 +0800
Subject: [PATCH 4/9] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E6=96=B0?=
=?UTF-8?q?=E5=A2=9E=E7=BC=93=E6=85=A2=E8=87=AA=E5=8A=A8=E5=B7=A1=E8=88=AA?=
=?UTF-8?q?=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/cruise2.py | 59 ++++++++++++++++++++++
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 60 insertions(+), 1 deletion(-)
create mode 100644 src/simple_wheeled_vehicle/cruise2.py
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 a0f375ecf0..8d75444fb4 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
-
+
From 577ec6cc153d68f339a817c1aed1f6ad6cef372c Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 02:19:33 +0800
Subject: [PATCH 5/9] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E6=96=B0?=
=?UTF-8?q?=E5=A2=9E=E7=BC=93=E6=85=A2=E8=87=AA=E5=8A=A8=E5=B7=A1=E8=88=AA?=
=?UTF-8?q?=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/cruise2.py | 59 ++++++++++++++++++++++
src/simple_wheeled_vehicle/wheeled_car.xml | 18 +++----
2 files changed, 67 insertions(+), 10 deletions(-)
create mode 100644 src/simple_wheeled_vehicle/cruise2.py
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 d446b6e1dc..a0f375ecf0 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -1,4 +1,4 @@
-
+
@@ -7,7 +7,6 @@
-
@@ -25,7 +24,7 @@
-
+
@@ -35,8 +34,7 @@
-
-
+
@@ -49,8 +47,6 @@
-
-
@@ -60,8 +56,6 @@
-
-
@@ -69,7 +63,11 @@
-
+
+
+
+
+
From 047f47dfc33e9b28ab9ff87a6fca7fd0195f229c Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 08:30:23 +0800
Subject: [PATCH 6/9] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E5=A2=9E?=
=?UTF-8?q?=E5=8A=A0=E9=81=87=E9=9A=9C=E5=81=9C=E6=AD=A2=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/3.py | 177 ++++++++++++++++++---
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 154 insertions(+), 25 deletions(-)
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/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index a0f375ecf0..58ae5399a3 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
-
+
From 8d77ab962421aed84829bd0fc99868472480c8dc Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 10:53:13 +0800
Subject: [PATCH 7/9] =?UTF-8?q?=E5=B0=8F=E8=BD=A6=E6=A3=80=E6=B5=8B?=
=?UTF-8?q?=E5=88=B0=E9=9A=9C=E7=A2=8D=E5=90=8E=E4=BC=9A=E8=87=AA=E5=8A=A8?=
=?UTF-8?q?=E8=BD=AC=E5=90=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/5.py | 283 +++++++++++++++++++++
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 284 insertions(+), 1 deletion(-)
create mode 100644 src/simple_wheeled_vehicle/5.py
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/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 58ae5399a3..7e233c71e1 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
-
+
From 8accd54a08148d7469942af0a459e73290889ef6 Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 11:35:06 +0800
Subject: [PATCH 8/9] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BB=A3=E7=A0=81?=
=?UTF-8?q?=EF=BC=8C=E4=BD=BF=E5=B0=8F=E8=BD=A6=E8=BD=AC=E5=90=91=E9=80=9F?=
=?UTF-8?q?=E5=BA=A6=E5=87=8F=E7=BC=93?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/6.py | 448 +++++++++++++++++++++
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 449 insertions(+), 1 deletion(-)
create mode 100644 src/simple_wheeled_vehicle/6.py
diff --git a/src/simple_wheeled_vehicle/6.py b/src/simple_wheeled_vehicle/6.py
new file mode 100644
index 0000000000..ea37034124
--- /dev/null
+++ b/src/simple_wheeled_vehicle/6.py
@@ -0,0 +1,448 @@
+"""
+自动巡航小车 - 带前方障碍物检测与智能路径规划
+- 巡航速度提高至0.003 m/s
+- 前方0.5米内有障碍物时停止并规划新路径
+- 避免摆头循环,寻找可行方向
+- 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)
+
+# ------------------- 参数设置 -------------------
+CRUISE_SPEED = 0.003
+TURN_SPEED = CRUISE_SPEED * 0.4
+OBSTACLE_DISTANCE_THRESHOLD = 0.7 # 增加检测距离
+SAFE_DISTANCE = 0.3
+TURN_ANGLE = 0.3
+TURN_DURATION = 60
+SCAN_RANGE = 1.0 # 扩大扫描范围
+
+# 路径规划参数
+MAX_TURN_ATTEMPTS = 3 # 最大转向尝试次数
+PATH_SCAN_ANGLES = [-60, -30, 0, 30, 60] # 扫描角度(度)
+PATH_SCAN_DISTANCE = 1.5 # 路径扫描距离
+
+# 障碍物名称列表
+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(direction_angle=0):
+ """检测指定方向是否有障碍物"""
+
+ # 获取小车当前位置
+ chassis_pos = data.body(chassis_body_id).xpos
+
+ # 获取小车当前速度方向
+ velocity = data.qvel[:2]
+ if np.linalg.norm(velocity) < 0.0001:
+ forward_direction = np.array([1.0, 0.0])
+ else:
+ forward_direction = velocity / np.linalg.norm(velocity)
+
+ # 旋转前向方向到指定角度
+ if direction_angle != 0:
+ cos_a = math.cos(direction_angle)
+ sin_a = math.sin(direction_angle)
+ rotated_direction = np.array([
+ forward_direction[0] * cos_a - forward_direction[1] * sin_a,
+ forward_direction[0] * sin_a + forward_direction[1] * cos_a
+ ])
+ forward_direction = rotated_direction
+
+ # 扫描所有障碍物
+ 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]
+ dy = obs_pos[1] - chassis_pos[1]
+
+ relative_pos = np.array([dx, dy])
+ distance = np.linalg.norm(relative_pos)
+
+ if distance > 0 and distance < SCAN_RANGE:
+ obstacle_direction = relative_pos / distance
+
+ dot_product = np.dot(obstacle_direction, forward_direction)
+ dot_product = max(-1.0, min(1.0, dot_product))
+ angle_diff = math.acos(dot_product)
+
+ if angle_diff < math.radians(45) and dx > -0.2:
+ cross_product = np.cross([forward_direction[0], forward_direction[1], 0],
+ [obstacle_direction[0], obstacle_direction[1], 0])
+ lateral_distance = abs(cross_product[2]) * distance
+
+ if angle_diff < math.radians(25) and lateral_distance < 0.5:
+ 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
+
+# ------------------- 路径扫描函数 -------------------
+def scan_path_directions():
+ """扫描多个方向的路径可行性"""
+
+ path_scores = []
+
+ for angle_deg in PATH_SCAN_ANGLES:
+ angle_rad = math.radians(angle_deg)
+
+ # 检查该方向是否有障碍物
+ obstacle_status, obstacle_distance, _ = check_front_obstacle(angle_rad)
+
+ # 计算分数:距离越远分数越高
+ if obstacle_status == 0:
+ # 无障碍物,给最高分
+ score = PATH_SCAN_DISTANCE
+ elif obstacle_status == 1:
+ # 有障碍物但距离较远
+ score = obstacle_distance
+ else:
+ # 有近距离障碍物
+ score = 0
+
+ path_scores.append((angle_rad, score, obstacle_status))
+
+ # 按分数排序
+ path_scores.sort(key=lambda x: x[1], reverse=True)
+
+ return path_scores
+
+# ------------------- 选择最佳路径 -------------------
+def choose_best_path():
+ """选择最佳路径方向"""
+
+ # 扫描所有方向
+ path_scores = scan_path_directions()
+
+ # 选择最佳方向
+ best_angle, best_score, best_status = path_scores[0]
+
+ # 如果所有方向都有障碍物,选择障碍物最远的方向
+ if best_score == 0:
+ # 后退方案
+ print("所有方向都有障碍物,尝试后退")
+ return None, "BACKUP"
+
+ # 如果最佳方向不是正前方,需要转向
+ if abs(best_angle) > math.radians(10):
+ if best_angle > 0:
+ return best_angle, f"左转{abs(math.degrees(best_angle)):.0f}度"
+ else:
+ return best_angle, f"右转{abs(math.degrees(best_angle)):.0f}度"
+ else:
+ return 0, "直行"
+
+# ------------------- 后退操作 -------------------
+def perform_backup(backup_counter):
+ """执行后退操作"""
+
+ if backup_counter < 30:
+ # 后退(负速度)
+ data.ctrl[0] = 0.0
+ data.ctrl[1] = 0.0
+ data.ctrl[2] = -TURN_SPEED * 0.5
+ data.ctrl[3] = -TURN_SPEED * 0.5
+ data.ctrl[4] = -TURN_SPEED * 0.5
+ data.ctrl[5] = -TURN_SPEED * 0.5
+ return False, backup_counter + 1
+ else:
+ # 后退完成
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+ return True, 0
+
+# ------------------- 主循环 -------------------
+mujoco.mj_resetData(model, data)
+
+# 状态变量
+CAR_STATE = "CRUISING" # CRUISING, DECELERATING, STOPPED, PATH_PLANNING, TURNING, SCANNING, RESUME, BACKING_UP
+turn_counter = 0
+turn_angle = 0
+turn_direction = ""
+scan_counter = 0
+deceleration_counter = 0
+current_speed = CRUISE_SPEED
+turn_attempts = 0 # 转向尝试次数
+backup_counter = 0
+
+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
+ scan_counter = 0
+ deceleration_counter = 0
+ current_speed = CRUISE_SPEED
+ turn_attempts = 0
+ backup_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
+ current_speed = 0
+
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+
+ elif obstacle_status == 1:
+ # 检测到障碍物,减速
+ CAR_STATE = "DECELERATING"
+ deceleration_counter = 0
+ print(f"\n⚠️ 检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),开始减速...")
+ 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
+ current_speed = CRUISE_SPEED
+
+ elif CAR_STATE == "DECELERATING":
+ # 减速状态
+ deceleration_counter += 1
+
+ decel_progress = min(1.0, deceleration_counter / 15.0)
+ current_speed = CRUISE_SPEED * (1.0 - decel_progress)
+
+ data.ctrl[0] = 0.0
+ data.ctrl[1] = 0.0
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+
+ if deceleration_counter > 20:
+ CAR_STATE = "STOPPED"
+ print("减速完成,准备规划路径")
+ turn_counter = 0
+
+ elif CAR_STATE == "STOPPED":
+ # 停止状态,准备路径规划
+ turn_counter += 1
+
+ current_speed = 0
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+
+ if turn_counter > 10:
+ print("正在规划路径...")
+ CAR_STATE = "PATH_PLANNING"
+ turn_counter = 0
+
+ elif CAR_STATE == "PATH_PLANNING":
+ # 路径规划状态
+ print("扫描可行路径...")
+
+ # 选择最佳路径
+ best_angle, path_direction = choose_best_path()
+
+ if path_direction == "BACKUP":
+ # 需要后退
+ print("所有方向都有障碍物,执行后退")
+ CAR_STATE = "BACKING_UP"
+ backup_counter = 0
+ else:
+ # 找到可行路径
+ turn_angle = best_angle
+ turn_direction = path_direction
+
+ # 检查是否需要大角度转向
+ if abs(turn_angle) > math.radians(45):
+ turn_attempts += 1
+ if turn_attempts >= MAX_TURN_ATTEMPTS:
+ print(f"多次尝试后选择大角度转向: {turn_direction}")
+
+ print(f"选择路径: {turn_direction}")
+ CAR_STATE = "TURNING"
+ turn_counter = 0
+
+ elif CAR_STATE == "BACKING_UP":
+ # 后退状态
+ backup_complete, backup_counter = perform_backup(backup_counter)
+
+ if backup_complete:
+ print("后退完成,重新规划路径")
+ CAR_STATE = "PATH_PLANNING"
+
+ elif CAR_STATE == "TURNING":
+ # 转向状态
+ turn_counter += 1
+
+ # 渐进式转向
+ progress = min(1.0, turn_counter / 10.0)
+ current_angle = turn_angle * progress
+
+ data.ctrl[0] = current_angle
+ data.ctrl[1] = current_angle
+
+ if turn_counter > 5:
+ speed_progress = min(1.0, (turn_counter - 5) / 15.0)
+ current_speed = TURN_SPEED * speed_progress
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+
+ if turn_counter % 15 == 0:
+ print(f"正在{turn_direction},角度: {math.degrees(current_angle):.1f}度")
+
+ if turn_counter > TURN_DURATION:
+ print(f"{turn_direction}完成,开始扫描前方...")
+ CAR_STATE = "SCANNING"
+ turn_counter = 0
+ scan_counter = 0
+
+ elif CAR_STATE == "SCANNING":
+ # 扫描状态
+ scan_counter += 1
+
+ data.ctrl[0] = turn_angle * 0.5
+ data.ctrl[1] = turn_angle * 0.5
+ data.ctrl[2] = TURN_SPEED * 0.6
+ data.ctrl[3] = TURN_SPEED * 0.6
+ data.ctrl[4] = TURN_SPEED * 0.6
+ data.ctrl[5] = TURN_SPEED * 0.6
+ current_speed = TURN_SPEED * 0.6
+
+ if scan_counter % 10 == 0:
+ obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+
+ if obstacle_status == 0:
+ print("前方安全,准备恢复巡航")
+ CAR_STATE = "RESUME"
+ turn_counter = 0
+ turn_attempts = 0 # 重置尝试次数
+ else:
+ print(f"转向后仍检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),重新规划")
+ CAR_STATE = "STOPPED"
+ turn_counter = 0
+
+ if scan_counter > 50:
+ print("扫描超时,尝试恢复巡航")
+ CAR_STATE = "RESUME"
+ turn_counter = 0
+
+ elif CAR_STATE == "RESUME":
+ # 恢复巡航状态
+ turn_counter += 1
+
+ progress = min(1.0, turn_counter / 15.0)
+
+ current_angle = turn_angle * (1.0 - progress)
+ data.ctrl[0] = current_angle
+ data.ctrl[1] = current_angle
+
+ current_speed = TURN_SPEED + (CRUISE_SPEED - TURN_SPEED) * progress
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+
+ if turn_counter > 20:
+ 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
+ current_speed = 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}度"
+
+ status_info += f", 尝试次数: {turn_attempts}"
+
+ 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/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 7e233c71e1..58ae5399a3 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
-
+
From 3783b20308598ae297e320e14931d1017fb9c0b9 Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 19:25:42 +0800
Subject: [PATCH 9/9] =?UTF-8?q?=E5=B0=8F=E8=BD=A6=E6=A3=80=E6=B5=8B?=
=?UTF-8?q?=E5=88=B0=E9=9A=9C=E7=A2=8D=E4=BC=9A=E8=87=AA=E5=8A=A8=E8=BD=AC?=
=?UTF-8?q?=E5=90=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/main.py | 296 +++++++++++++++++----
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 251 insertions(+), 47 deletions(-)
diff --git a/src/simple_wheeled_vehicle/main.py b/src/simple_wheeled_vehicle/main.py
index f486ce457b..457a4b4b57 100644
--- a/src/simple_wheeled_vehicle/main.py
+++ b/src/simple_wheeled_vehicle/main.py
@@ -1,79 +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.Key.up: False,
- keyboard.Key.down: False,
- keyboard.Key.left: False,
- keyboard.Key.right: False,
- keyboard.KeyCode.from_char('r'): False}
+# ------------------- 键盘监听 -------------------
+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)
-# ---------------- 参数 ------------------
-MAX_SPEED = 0.4 # ← 封顶速度(m/s)
-ACCEL_RAMP = 0.02 # ← 爬升系数
-steer_target = 0.0
-speed_target = 0.0
-speed = 0.0 # 当前速度
+# ------------------- 参数设置 -------------------
+# 提高两倍的速度: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("\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:
+ # 计算障碍物相对于小车前向方向的横向偏移
+ 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():
- if KEYS[keyboard.KeyCode.from_char('r')]:
+ # 检查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 KEYS[keyboard.Key.up]:
- speed_target = min(speed_target + ACCEL_RAMP, MAX_SPEED)
- elif KEYS[keyboard.Key.down]:
- speed_target = max(speed_target - ACCEL_RAMP, -MAX_SPEED * 0.7)
- else:
- speed_target = 0.0
+ # 根据当前状态执行不同操作
+ if CAR_STATE == "CRUISING":
+ # 巡航状态:检测前方障碍物
+ obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
- # ---- 顺滑转向:只给目标角度,伺服自动慢回正 ----
- if KEYS[keyboard.Key.left]:
- steer_target = 0.4 # 最大 0.4 rad
- elif KEYS[keyboard.Key.right]:
- steer_target = -0.4
- else:
- steer_target = 0.0 # 松键即回正(伺服自带阻尼)
- # ---- 速度斜坡 ----
- speed = speed_target # 电机直接吃斜坡值即可(已很柔)
-
- # ---- 输出 ----
- data.ctrl[0] = steer_target # 前左转向
- data.ctrl[1] = steer_target # 前右转向
- data.ctrl[2] = speed - steer_target * 0.3
- data.ctrl[3] = speed + steer_target * 0.3
- data.ctrl[4] = speed - steer_target * 0.15
- data.ctrl[5] = speed + steer_target * 0.15
-
- # ---- 仿真 + HUD ----
+ 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])
- print(f"\rspeed: {vel:5.2f} m/s", end='', flush=True)
+
+ # 获取当前转向角度
+ 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()
\ No newline at end of file
+print("\n程序结束")
\ 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 58ae5399a3..908aadffe3 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
-
+