From 78f3ee554c53dfc3ce5b2abed98bcd20eff7f4e7 Mon Sep 17 00:00:00 2001 From: windows <2877519694@qq.com> Date: Sun, 21 Dec 2025 11:21:07 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E5=BB=BA=E7=AB=8Bmujoco?= =?UTF-8?q?=E7=AE=80=E6=98=93=E5=B0=8F=E8=BD=A6=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/simple_wheeled_vehicle/1.py | 86 ++++++++++++++++++ src/simple_wheeled_vehicle/wheeled_car.xml | 101 +++++++++++++++++++++ 2 files changed, 187 insertions(+) create mode 100644 src/simple_wheeled_vehicle/1.py create mode 100644 src/simple_wheeled_vehicle/wheeled_car.xml diff --git a/src/simple_wheeled_vehicle/1.py b/src/simple_wheeled_vehicle/1.py new file mode 100644 index 0000000000..2d1eb5d0d5 --- /dev/null +++ b/src/simple_wheeled_vehicle/1.py @@ -0,0 +1,86 @@ +""" +超低速 + 顺滑转向小车 +- 速度 0.4 m/s 封顶 +- 转向:方向键只给“目标角度”,伺服自带阻尼慢回正 +- R 复位 +""" +import mujoco +import mujoco.viewer +import numpy as np +from pynput import keyboard + +# ---------------- 键盘 ------------------ +KEYS = {keyboard.Key.up: False, + keyboard.Key.down: False, + keyboard.Key.left: False, + keyboard.Key.right: False, + 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 # 当前速度 + +# ---------------- 复位 ------------------ +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 + + # ---- 超低速软启动 ---- + 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 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 ---- + mujoco.mj_step(model, data) + vel = np.linalg.norm(data.qvel[:3]) + print(f"\rspeed: {vel:5.2f} 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 new file mode 100644 index 0000000000..ff04a82bfa --- /dev/null +++ b/src/simple_wheeled_vehicle/wheeled_car.xml @@ -0,0 +1,101 @@ + + \ No newline at end of file From 8c04d655cd063f117fdc90facc2820446968bbeb Mon Sep 17 00:00:00 2001 From: windows <2877519694@qq.com> Date: Sun, 21 Dec 2025 11:46:50 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E5=BB=BA=E7=AB=8Bmujoco?= =?UTF-8?q?=E7=AE=80=E6=98=93=E5=B0=8F=E8=BD=A6=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/simple_wheeled_vehicle/1.py | 1 - src/simple_wheeled_vehicle/wheeled_car.xml | 20 +++++++++----------- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/simple_wheeled_vehicle/1.py b/src/simple_wheeled_vehicle/1.py index 2d1eb5d0d5..0023f24196 100644 --- a/src/simple_wheeled_vehicle/1.py +++ b/src/simple_wheeled_vehicle/1.py @@ -65,7 +65,6 @@ def reset_car(): steer_target = -0.4 else: steer_target = 0.0 # 松键即回正(伺服自带阻尼) - # ---- 速度斜坡 ---- speed = speed_target # 电机直接吃斜坡值即可(已很柔) diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml index ff04a82bfa..3e91223e18 100644 --- a/src/simple_wheeled_vehicle/wheeled_car.xml +++ b/src/simple_wheeled_vehicle/wheeled_car.xml @@ -14,34 +14,33 @@ - - - - + + - + - + + + + - - - + @@ -54,7 +53,7 @@ - + @@ -83,7 +82,6 @@ - From ca5f4f12ac645ba8320a87940faecc38a0b4832a Mon Sep 17 00:00:00 2001 From: windows <2877519694@qq.com> Date: Sun, 21 Dec 2025 11:59:21 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BB=A3=E7=A0=81?= =?UTF-8?q?=E4=BD=BF=E5=B0=8F=E8=BD=A6=E8=83=BD=E5=A4=9F=E8=87=AA=E5=8A=A8?= =?UTF-8?q?=E5=B7=A1=E8=88=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/simple_wheeled_vehicle/2.py | 60 ++++++++++++++++++++++ src/simple_wheeled_vehicle/wheeled_car.xml | 1 - 2 files changed, 60 insertions(+), 1 deletion(-) create mode 100644 src/simple_wheeled_vehicle/2.py diff --git a/src/simple_wheeled_vehicle/2.py b/src/simple_wheeled_vehicle/2.py new file mode 100644 index 0000000000..72e5a0d308 --- /dev/null +++ b/src/simple_wheeled_vehicle/2.py @@ -0,0 +1,60 @@ +""" +自动巡航小车 +- 恒定速度 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 3e91223e18..1742d145ce 100644 --- a/src/simple_wheeled_vehicle/wheeled_car.xml +++ b/src/simple_wheeled_vehicle/wheeled_car.xml @@ -87,7 +87,6 @@ -