Skip to content
Closed
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
86 changes: 86 additions & 0 deletions src/simple_wheeled_vehicle/1.py
Original file line number Diff line number Diff line change
@@ -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()
101 changes: 101 additions & 0 deletions src/simple_wheeled_vehicle/wheeled_car.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
<mujoco model="slow_smooth_car">
<option timestep="0.002" integrator="Euler"/>
<compiler angle="radian" coordinate="local" autolimits="false"/>

<asset>
<texture name="wheel" type="2d" builtin="checker" rgb1="0.1 0.1 0.1" rgb2="0.2 0.2 0.2" width="128" height="128"/>
<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"/>
</asset>

<default>
<default class="wheel">
<geom type="sphere" size="0.04" material="wheel"
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"
range="-0.02 0.04" limited="true"/>
</default>
<!-- 伺服:小齿轮 = 慢速回正,阻尼 = 顺滑 -->
<default class="servo">
<motor ctrlrange="-0.3 0.3" ctrllimited="true" gear="15" damping="10"/>
</default>
<!-- 驱动扭矩再减半 -->
<default class="drive">
<motor ctrlrange="-0.8 0.8" ctrllimited="true" gear="25"/>
</default>
</default>

<worldbody>
<geom name="floor" type="plane" size="10 10 0.1" material="grid"/>

<!-- 重心低于轮轴 -->
<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"/>

<!-- 轮距 0.34 -->
<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="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"/>
<body name="fl_wheel" pos="0 0 -0.04">
<joint name="fl_drive" type="hinge" axis="0 1 0" damping="0.1"/>
<geom class="wheel" pos="0 0 0"/>
</body>
</body>
</body>

<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="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"/>
<body name="fr_wheel" pos="0 0 -0.04">
<joint name="fr_drive" type="hinge" axis="0 1 0" damping="0.1"/>
<geom class="wheel" pos="0 0 0"/>
</body>
</body>
</body>

<body name="rear_left" pos="-0.16 0.17 0">
<inertial pos="0 0 0" mass="0.05" diaginertia="1e-4 1e-4 1e-4"/>
<joint name="rl_arm_hinge" axis="0 1 0" damping="12" stiffness="60" range="-0.02 0.04" limited="true"/>
<body name="rl_wheel" pos="0 0 -0.04">
<joint name="rl_drive" type="hinge" axis="0 1 0" damping="0.1"/>
<geom class="wheel" pos="0 0 0"/>
</body>
</body>

<body name="rear_right" pos="-0.16 -0.17 0">
<inertial pos="0 0 0" mass="0.05" diaginertia="1e-4 1e-4 1e-4"/>
<joint name="rr_arm_hinge" axis="0 1 0" damping="12" stiffness="60" range="-0.02 0.04" limited="true"/>
<body name="rr_wheel" pos="0 0 -0.04">
<joint name="rr_drive" type="hinge" axis="0 1 0" damping="0.1"/>
<geom class="wheel" pos="0 0 0"/>
</body>
</body>

<!-- 相机 -->
<body name="cam" pos="-0.3 0 0.15" quat="0.819 0.573 0 0">
<camera name="track" mode="track" pos="0 0 0" xyaxes="1 0 0 0 0 1" fovy="50"/>
</body>
</body>
</worldbody>

<actuator>
<motor name="steer_fl" joint="fl_steer" class="servo"/>
<motor name="steer_fr" joint="fr_steer" class="servo"/>
<motor name="drive_fl" joint="fl_drive" class="drive"/>
<motor name="drive_fr" joint="fr_drive" class="drive"/>
<motor name="drive_rl" joint="rl_drive" class="drive"/>
<motor name="drive_rr" joint="rr_drive" class="drive"/>
</actuator>
</mujoco>
Loading