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
85 changes: 85 additions & 0 deletions src/simple_wheeled_vehicle/1.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
"""
超低速 + 顺滑转向小车
- 速度 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()
60 changes: 60 additions & 0 deletions src/simple_wheeled_vehicle/2.py
Original file line number Diff line number Diff line change
@@ -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()
79 changes: 79 additions & 0 deletions src/simple_wheeled_vehicle/main.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
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()
99 changes: 99 additions & 0 deletions src/simple_wheeled_vehicle/wheeled_car.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
<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="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"/>
</default>
<default class="servo">
<motor ctrlrange="-0.3 0.3" ctrllimited="true" gear="15"/>
</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"/>

<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"/>
<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="steer"/>
<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