Skip to content
Merged
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
1 change: 0 additions & 1 deletion src/simple_wheeled_vehicle/2.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()

# ---------------- 加载模型 --------------
Expand Down
59 changes: 59 additions & 0 deletions src/simple_wheeled_vehicle/3.py
Original file line number Diff line number Diff line change
@@ -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()
49 changes: 43 additions & 6 deletions src/simple_wheeled_vehicle/wheeled_car.xml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<mujoco model="slow_smooth_car">
<mujoco model="auto_creep_with_obstacles">
<option timestep="0.002" integrator="Euler"/>
<compiler angle="radian" coordinate="local" autolimits="false"/>

Expand All @@ -7,6 +7,9 @@
<material name="wheel" texture="wheel" texuniform="true" reflectance="0.2"/>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1="0.6 0.6 0.6" rgb2="0.8 0.8 0.8"/>
<material name="grid" texture="grid" texuniform="true" reflectance="0.2"/>
<!-- 障碍物材质 -->
<material name="obs" rgba="0.2 0.4 0.8 1"/>
<material name="wall" rgba="0.5 0.5 0.5 1"/>
</asset>

<default>
Expand All @@ -15,15 +18,14 @@
mass="0.25" friction="1.0 0.005 0.005"/>
</default>
<default class="arm">
<joint type="hinge" axis="0 1 0" damping="15" stiffness="60"
<joint type="hinge" axis="0 1 0" damping="12" stiffness="60"
range="-0.02 0.04" limited="true"/>
</default>
<default class="steer">
<joint type="hinge" axis="0 0 1" damping="5" limited="false"/>
</default>
<!-- 电机:去掉 damping,仅用 ctrlrange 限扭 -->
<default class="drive">
<motor ctrlrange="-1.2 1.2" ctrllimited="true" gear="30"/>
<motor ctrlrange="-0.8 0.8" ctrllimited="true" gear="25"/>
</default>
<default class="servo">
<motor ctrlrange="-0.3 0.3" ctrllimited="true" gear="15"/>
Expand All @@ -33,14 +35,49 @@
<worldbody>
<geom name="floor" type="plane" size="10 10 0.1" material="grid"/>

<!-- ===== 周边障碍物群(5×5 m 范围) ===== -->
<!-- 1. 盒子障碍 -->
<body name="obs_box1" pos="1.0 0.5 0.05">
<geom type="box" size="0.15 0.15 0.15" material="obs" mass="1"/>
</body>
<body name="obs_box2" pos="-1.2 1.2 0.05">
<geom type="box" size="0.12 0.12 0.12" material="obs" mass="0.8"/>
</body>
<body name="obs_box3" pos="0 -1.8 0.05">
<geom type="box" size="0.18 0.18 0.18" material="obs" mass="1.2"/>
</body>
<body name="obs_box4" pos="2.0 -1.0 0.05">
<geom type="box" size="0.10 0.20 0.10" material="obs" mass="0.9"/>
</body>

<!-- 2. 球障碍 -->
<body name="obs_ball1" pos="-1.0 -1.0 0.04">
<geom type="sphere" size="0.08" material="obs" mass="0.6"/>
</body>
<body name="obs_ball2" pos="2.2 1.3 0.04">
<geom type="sphere" size="0.06" material="obs" mass="0.4"/>
</body>
<body name="obs_ball3" pos="-2.0 0.5 0.04">
<geom type="sphere" size="0.10" material="obs" mass="0.8"/>
</body>

<!-- 3. 矮墙障碍(当路障) -->
<body name="wall1" pos="0 2.5 0.02">
<geom type="box" size="0.8 0.05 0.04" material="wall" mass="0.5"/>
</body>
<body name="wall2" pos="-2.5 -0.5 0.02">
<geom type="box" size="0.05 0.8 0.04" material="wall" mass="0.5"/>
</body>

<!-- 小车 -->
<body name="chassis" pos="0 0 0.03">
<freejoint/>
<geom name="body" type="box" size="0.18 0.22 0.02" pos="0 0 -0.02" rgba="0.9 0.2 0.2 1" mass="3"/>
<geom type="box" size="0.18 0.22 0.04" rgba="0.9 0.2 0.2 1" contype="0" conaffinity="0"/>

<body name="front_left_hub" pos="0.16 0.17 0">
<inertial pos="0 0 0" mass="0.05" diaginertia="1e-4 1e-4 1e-4"/>
<joint name="fl_steer" class="steer"/>
<joint name="fl_steer" class="servo"/>
<body name="fl_arm" pos="0 0 0">
<inertial pos="0 0 0" mass="0.05" diaginertia="1e-4 1e-4 1e-4"/>
<joint name="fl_arm_hinge" axis="0 1 0" damping="12" stiffness="60" range="-0.02 0.04" limited="true"/>
Expand All @@ -53,7 +90,7 @@

<body name="front_right_hub" pos="0.16 -0.17 0">
<inertial pos="0 0 0" mass="0.05" diaginertia="1e-4 1e-4 1e-4"/>
<joint name="fr_steer" class="steer"/>
<joint name="fr_steer" class="servo"/>
<body name="fr_arm" pos="0 0 0">
<inertial pos="0 0 0" mass="0.05" diaginertia="1e-4 1e-4 1e-4"/>
<joint name="fr_arm_hinge" axis="0 1 0" damping="12" stiffness="60" range="-0.02 0.04" limited="true"/>
Expand Down
Loading