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
7 changes: 7 additions & 0 deletions src/minicarsim/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,8 @@ def run_simulation(self):
# 查找车辆的驱动关节
rear_left_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rear_left_wheel_motor")
rear_right_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rear_right_wheel_motor")
front_left_steer_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "front_left_steering")
front_right_steer_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "front_right_steering")

if rear_left_idx >= 0 and rear_right_idx >= 0:
print("找到了车辆驱动关节")
Expand All @@ -213,6 +215,11 @@ def run_simulation(self):
self.data.ctrl[rear_left_idx] = 5.0 # 左后轮速度
if rear_right_idx >= 0:
self.data.ctrl[rear_right_idx] = 5.0 # 右后轮速度
# 设置前轮转向
if front_left_steer_idx >= 0:
self.data.ctrl[front_left_steer_idx] = np.sin(i * 0.05) * 10 # 正弦变化转向
if front_right_steer_idx >= 0:
self.data.ctrl[front_right_steer_idx] = np.sin(i * 0.05) * 10 # 正弦变化转向

# 执行仿真步长
mujoco.mj_step(self.model, self.data)
Expand Down
102 changes: 102 additions & 0 deletions src/minicarsim/models/simple_car.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@
<mujoco model="simple_car">
<compiler angle="radian" inertiafromgeom="true"/>

<option timestep="0.005" gravity="0 0 -9.81" iterations="200" solver="Newton" cone="elliptic" impratio="5" viscosity="0.1"/>

<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.871 0.616 0.4 0.5"/>
<global azimuth="-130" elevation="-20"/>
</visual>

<asset>
<texture builtin="gradient" height="100" rgb1="0.9 0.9 0.9" rgb2="0.549 0.251 0.0" type="skybox" width="100"/>
<texture name="texplane" type="2d" builtin="checker" rgb1="0.5 0.5 0.5" rgb2="0.55 0.55 0.55" width="100" height="100"/>
<material name="MatPlane" reflectance="0.0" texture="texplane" texrepeat="1 1" texuniform="true"/>
<material name="car_body" rgba="0.2 0.2 0.8 1"/>
<material name="car_wheels" rgba="0.1 0.1 0.1 1"/>
<material name="obstacle_red" rgba="0.8 0.2 0.2 1"/>
<material name="obstacle_green" rgba="0.2 0.8 0.2 1"/>
<material name="obstacle_blue" rgba="0.2 0.2 0.8 1"/>
<material name="obstacle_yellow" rgba="0.8 0.8 0.2 1"/>
</asset>

<worldbody>
<!-- Ground plane -->
<geom name="floor" friction="2.0 .1 .1" pos="0 0 0" size="0 0 0.125" type="plane" material="MatPlane" condim="3" conaffinity="1" contype="1" group="2" rgba="0.8 0.9 0.8 1" solref="0.08 1"/>
<light cutoff="1000" diffuse="1.5 1.5 1.5" dir="-0 0 -1.3" directional="true" exponent="10" pos="0 0 10.3" specular=".1 .1 .1" castshadow="false"/>

<!-- Simple car body -->
<body name="vehicle" pos="0 0 0.3">
<freejoint name="vehicle_joint"/>

<!-- Car chassis -->
<geom name="chassis" type="box" size="1 0.5 0.2" material="car_body" mass="10" friction="0.2 0.1 0.1" solref="0.08 1"/>

<!-- Front wheels -->
<body name="front_left_wheel" pos="0.8 0.6 -0.1">
<joint name="front_left_wheel_joint" type="hinge" axis="0 1 0" limited="false"/>
<geom name="front_left_wheel_geom" type="sphere" size="0.2" material="car_wheels" mass="1.0" friction="2.0 0.1 0.1" solref="0.08 1"/>
</body>

<body name="front_right_wheel" pos="0.8 -0.6 -0.1">
<joint name="front_right_wheel_joint" type="hinge" axis="0 1 0" limited="false"/>
<geom name="front_right_wheel_geom" type="sphere" size="0.2" material="car_wheels" mass="1.0" friction="2.0 0.1 0.1" solref="0.08 1"/>
</body>

<!-- Rear wheels -->
<body name="rear_left_wheel" pos="-0.8 0.6 -0.1">
<joint name="rear_left_wheel_joint" type="hinge" axis="0 1 0" limited="false"/>
<geom name="rear_left_wheel_geom" type="sphere" size="0.2" material="car_wheels" mass="1.0" friction="2.0 0.1 0.1" solref="0.08 1"/>
</body>

<body name="rear_right_wheel" pos="-0.8 -0.6 -0.1">
<joint name="rear_right_wheel_joint" type="hinge" axis="0 1 0" limited="false"/>
<geom name="rear_right_wheel_geom" type="sphere" size="0.2" material="car_wheels" mass="1.0" friction="2.0 0.1 0.1" solref="0.08 1"/>
</body>

<!-- LiDAR sensor -->
<site name="lidar_site" pos="0 0 0.8" type="sphere" size="0.05"/>

<!-- Camera sensor -->
<site name="camera_site" pos="1.2 0 0.7" type="sphere" size="0.05"/>
</body>

<!-- More obstacles for object detection -->
<body name="obstacle1" pos="5 2 0.3">
<geom name="obstacle1_geom" type="box" size="0.5 0.5 0.5" material="obstacle_red" mass="5"/>
</body>

<body name="obstacle2" pos="8 -3 0.3">
<geom name="obstacle2_geom" type="box" size="0.5 0.5 0.5" material="obstacle_green" mass="5"/>
</body>

<body name="obstacle3" pos="-3 4 0.3">
<geom name="obstacle3_geom" type="box" size="0.5 0.5 0.5" material="obstacle_blue" mass="5"/>
</body>

<body name="obstacle4" pos="10 5 0.3">
<geom name="obstacle4_geom" type="box" size="0.5 0.5 0.5" material="obstacle_yellow" mass="5"/>
</body>

<body name="obstacle5" pos="-5 -2 0.3">
<geom name="obstacle5_geom" type="box" size="0.5 0.5 0.5" material="obstacle_red" mass="5"/>
</body>
</worldbody>

<actuator>
<!-- Motor for rear wheels -->
<velocity name="rear_left_wheel_motor" joint="rear_left_wheel_joint" kv="5.0"/>
<velocity name="rear_right_wheel_motor" joint="rear_right_wheel_joint" kv="5.0"/>

<!-- Steering for front wheels -->
<position name="front_left_steering" joint="front_left_wheel_joint" kp="15"/>
<position name="front_right_steering" joint="front_right_wheel_joint" kp="15"/>
</actuator>

<sensor>
<!-- Accelerometer and Gyro for IMU data -->
<accelerometer name="accelerometer" site="lidar_site"/>
<gyro name="gyro" site="lidar_site"/>
</sensor>
</mujoco>
Loading