mirror of
https://github.com/Farama-Foundation/Gymnasium.git
synced 2025-08-01 22:11:25 +00:00
314 lines
10 KiB
Python
314 lines
10 KiB
Python
"""
|
|
Top-down car dynamics simulation.
|
|
|
|
Some ideas are taken from this great tutorial http://www.iforce2d.net/b2dtut/top-down-car by Chris Campbell.
|
|
This simulation is a bit more detailed, with wheels rotation.
|
|
|
|
Created by Oleg Klimov. Licensed on the same terms as the rest of OpenAI Gym.
|
|
"""
|
|
|
|
import numpy as np
|
|
import math
|
|
import Box2D
|
|
from Box2D.b2 import (
|
|
edgeShape,
|
|
circleShape,
|
|
fixtureDef,
|
|
polygonShape,
|
|
revoluteJointDef,
|
|
contactListener,
|
|
shape,
|
|
)
|
|
|
|
SIZE = 0.02
|
|
ENGINE_POWER = 100000000 * SIZE * SIZE
|
|
WHEEL_MOMENT_OF_INERTIA = 4000 * SIZE * SIZE
|
|
FRICTION_LIMIT = (
|
|
1000000 * SIZE * SIZE
|
|
) # friction ~= mass ~= size^2 (calculated implicitly using density)
|
|
WHEEL_R = 27
|
|
WHEEL_W = 14
|
|
WHEELPOS = [(-55, +80), (+55, +80), (-55, -82), (+55, -82)]
|
|
HULL_POLY1 = [(-60, +130), (+60, +130), (+60, +110), (-60, +110)]
|
|
HULL_POLY2 = [(-15, +120), (+15, +120), (+20, +20), (-20, 20)]
|
|
HULL_POLY3 = [
|
|
(+25, +20),
|
|
(+50, -10),
|
|
(+50, -40),
|
|
(+20, -90),
|
|
(-20, -90),
|
|
(-50, -40),
|
|
(-50, -10),
|
|
(-25, +20),
|
|
]
|
|
HULL_POLY4 = [(-50, -120), (+50, -120), (+50, -90), (-50, -90)]
|
|
WHEEL_COLOR = (0.0, 0.0, 0.0)
|
|
WHEEL_WHITE = (0.3, 0.3, 0.3)
|
|
MUD_COLOR = (0.4, 0.4, 0.0)
|
|
|
|
|
|
class Car:
|
|
def __init__(self, world, init_angle, init_x, init_y):
|
|
self.world = world
|
|
self.hull = self.world.CreateDynamicBody(
|
|
position=(init_x, init_y),
|
|
angle=init_angle,
|
|
fixtures=[
|
|
fixtureDef(
|
|
shape=polygonShape(
|
|
vertices=[(x * SIZE, y * SIZE) for x, y in HULL_POLY1]
|
|
),
|
|
density=1.0,
|
|
),
|
|
fixtureDef(
|
|
shape=polygonShape(
|
|
vertices=[(x * SIZE, y * SIZE) for x, y in HULL_POLY2]
|
|
),
|
|
density=1.0,
|
|
),
|
|
fixtureDef(
|
|
shape=polygonShape(
|
|
vertices=[(x * SIZE, y * SIZE) for x, y in HULL_POLY3]
|
|
),
|
|
density=1.0,
|
|
),
|
|
fixtureDef(
|
|
shape=polygonShape(
|
|
vertices=[(x * SIZE, y * SIZE) for x, y in HULL_POLY4]
|
|
),
|
|
density=1.0,
|
|
),
|
|
],
|
|
)
|
|
self.hull.color = (0.8, 0.0, 0.0)
|
|
self.wheels = []
|
|
self.fuel_spent = 0.0
|
|
WHEEL_POLY = [
|
|
(-WHEEL_W, +WHEEL_R),
|
|
(+WHEEL_W, +WHEEL_R),
|
|
(+WHEEL_W, -WHEEL_R),
|
|
(-WHEEL_W, -WHEEL_R),
|
|
]
|
|
for wx, wy in WHEELPOS:
|
|
front_k = 1.0 if wy > 0 else 1.0
|
|
w = self.world.CreateDynamicBody(
|
|
position=(init_x + wx * SIZE, init_y + wy * SIZE),
|
|
angle=init_angle,
|
|
fixtures=fixtureDef(
|
|
shape=polygonShape(
|
|
vertices=[
|
|
(x * front_k * SIZE, y * front_k * SIZE)
|
|
for x, y in WHEEL_POLY
|
|
]
|
|
),
|
|
density=0.1,
|
|
categoryBits=0x0020,
|
|
maskBits=0x001,
|
|
restitution=0.0,
|
|
),
|
|
)
|
|
w.wheel_rad = front_k * WHEEL_R * SIZE
|
|
w.color = WHEEL_COLOR
|
|
w.gas = 0.0
|
|
w.brake = 0.0
|
|
w.steer = 0.0
|
|
w.phase = 0.0 # wheel angle
|
|
w.omega = 0.0 # angular velocity
|
|
w.skid_start = None
|
|
w.skid_particle = None
|
|
rjd = revoluteJointDef(
|
|
bodyA=self.hull,
|
|
bodyB=w,
|
|
localAnchorA=(wx * SIZE, wy * SIZE),
|
|
localAnchorB=(0, 0),
|
|
enableMotor=True,
|
|
enableLimit=True,
|
|
maxMotorTorque=180 * 900 * SIZE * SIZE,
|
|
motorSpeed=0,
|
|
lowerAngle=-0.4,
|
|
upperAngle=+0.4,
|
|
)
|
|
w.joint = self.world.CreateJoint(rjd)
|
|
w.tiles = set()
|
|
w.userData = w
|
|
self.wheels.append(w)
|
|
self.drawlist = self.wheels + [self.hull]
|
|
self.particles = []
|
|
|
|
def gas(self, gas):
|
|
"""control: rear wheel drive
|
|
|
|
Args:
|
|
gas (float): How much gas gets applied. Gets clipped between 0 and 1.
|
|
"""
|
|
gas = np.clip(gas, 0, 1)
|
|
for w in self.wheels[2:4]:
|
|
diff = gas - w.gas
|
|
if diff > 0.1:
|
|
diff = 0.1 # gradually increase, but stop immediately
|
|
w.gas += diff
|
|
|
|
def brake(self, b):
|
|
"""control: brake
|
|
|
|
Args:
|
|
b (0..1): Degree to which the brakes are applied. More than 0.9 blocks the wheels to zero rotation"""
|
|
for w in self.wheels:
|
|
w.brake = b
|
|
|
|
def steer(self, s):
|
|
"""control: steer
|
|
|
|
Args:
|
|
s (-1..1): target position, it takes time to rotate steering wheel from side-to-side"""
|
|
self.wheels[0].steer = s
|
|
self.wheels[1].steer = s
|
|
|
|
def step(self, dt):
|
|
for w in self.wheels:
|
|
# Steer each wheel
|
|
dir = np.sign(w.steer - w.joint.angle)
|
|
val = abs(w.steer - w.joint.angle)
|
|
w.joint.motorSpeed = dir * min(50.0 * val, 3.0)
|
|
|
|
# Position => friction_limit
|
|
grass = True
|
|
friction_limit = FRICTION_LIMIT * 0.6 # Grass friction if no tile
|
|
for tile in w.tiles:
|
|
friction_limit = max(
|
|
friction_limit, FRICTION_LIMIT * tile.road_friction
|
|
)
|
|
grass = False
|
|
|
|
# Force
|
|
forw = w.GetWorldVector((0, 1))
|
|
side = w.GetWorldVector((1, 0))
|
|
v = w.linearVelocity
|
|
vf = forw[0] * v[0] + forw[1] * v[1] # forward speed
|
|
vs = side[0] * v[0] + side[1] * v[1] # side speed
|
|
|
|
# WHEEL_MOMENT_OF_INERTIA*np.square(w.omega)/2 = E -- energy
|
|
# WHEEL_MOMENT_OF_INERTIA*w.omega * domega/dt = dE/dt = W -- power
|
|
# domega = dt*W/WHEEL_MOMENT_OF_INERTIA/w.omega
|
|
|
|
# add small coef not to divide by zero
|
|
w.omega += (
|
|
dt
|
|
* ENGINE_POWER
|
|
* w.gas
|
|
/ WHEEL_MOMENT_OF_INERTIA
|
|
/ (abs(w.omega) + 5.0)
|
|
)
|
|
self.fuel_spent += dt * ENGINE_POWER * w.gas
|
|
|
|
if w.brake >= 0.9:
|
|
w.omega = 0
|
|
elif w.brake > 0:
|
|
BRAKE_FORCE = 15 # radians per second
|
|
dir = -np.sign(w.omega)
|
|
val = BRAKE_FORCE * w.brake
|
|
if abs(val) > abs(w.omega):
|
|
val = abs(w.omega) # low speed => same as = 0
|
|
w.omega += dir * val
|
|
w.phase += w.omega * dt
|
|
|
|
vr = w.omega * w.wheel_rad # rotating wheel speed
|
|
f_force = -vf + vr # force direction is direction of speed difference
|
|
p_force = -vs
|
|
|
|
# Physically correct is to always apply friction_limit until speed is equal.
|
|
# But dt is finite, that will lead to oscillations if difference is already near zero.
|
|
|
|
# Random coefficient to cut oscillations in few steps (have no effect on friction_limit)
|
|
f_force *= 205000 * SIZE * SIZE
|
|
p_force *= 205000 * SIZE * SIZE
|
|
force = np.sqrt(np.square(f_force) + np.square(p_force))
|
|
|
|
# Skid trace
|
|
if abs(force) > 2.0 * friction_limit:
|
|
if (
|
|
w.skid_particle
|
|
and w.skid_particle.grass == grass
|
|
and len(w.skid_particle.poly) < 30
|
|
):
|
|
w.skid_particle.poly.append((w.position[0], w.position[1]))
|
|
elif w.skid_start is None:
|
|
w.skid_start = w.position
|
|
else:
|
|
w.skid_particle = self._create_particle(
|
|
w.skid_start, w.position, grass
|
|
)
|
|
w.skid_start = None
|
|
else:
|
|
w.skid_start = None
|
|
w.skid_particle = None
|
|
|
|
if abs(force) > friction_limit:
|
|
f_force /= force
|
|
p_force /= force
|
|
force = friction_limit # Correct physics here
|
|
f_force *= force
|
|
p_force *= force
|
|
|
|
w.omega -= dt * f_force * w.wheel_rad / WHEEL_MOMENT_OF_INERTIA
|
|
|
|
w.ApplyForceToCenter(
|
|
(
|
|
p_force * side[0] + f_force * forw[0],
|
|
p_force * side[1] + f_force * forw[1],
|
|
),
|
|
True,
|
|
)
|
|
|
|
def draw(self, viewer, draw_particles=True):
|
|
if draw_particles:
|
|
for p in self.particles:
|
|
viewer.draw_polyline(p.poly, color=p.color, linewidth=5)
|
|
for obj in self.drawlist:
|
|
for f in obj.fixtures:
|
|
trans = f.body.transform
|
|
path = [trans * v for v in f.shape.vertices]
|
|
viewer.draw_polygon(path, color=obj.color)
|
|
if "phase" not in obj.__dict__:
|
|
continue
|
|
a1 = obj.phase
|
|
a2 = obj.phase + 1.2 # radians
|
|
s1 = math.sin(a1)
|
|
s2 = math.sin(a2)
|
|
c1 = math.cos(a1)
|
|
c2 = math.cos(a2)
|
|
if s1 > 0 and s2 > 0:
|
|
continue
|
|
if s1 > 0:
|
|
c1 = np.sign(c1)
|
|
if s2 > 0:
|
|
c2 = np.sign(c2)
|
|
white_poly = [
|
|
(-WHEEL_W * SIZE, +WHEEL_R * c1 * SIZE),
|
|
(+WHEEL_W * SIZE, +WHEEL_R * c1 * SIZE),
|
|
(+WHEEL_W * SIZE, +WHEEL_R * c2 * SIZE),
|
|
(-WHEEL_W * SIZE, +WHEEL_R * c2 * SIZE),
|
|
]
|
|
viewer.draw_polygon([trans * v for v in white_poly], color=WHEEL_WHITE)
|
|
|
|
def _create_particle(self, point1, point2, grass):
|
|
class Particle:
|
|
pass
|
|
|
|
p = Particle()
|
|
p.color = WHEEL_COLOR if not grass else MUD_COLOR
|
|
p.ttl = 1
|
|
p.poly = [(point1[0], point1[1]), (point2[0], point2[1])]
|
|
p.grass = grass
|
|
self.particles.append(p)
|
|
while len(self.particles) > 30:
|
|
self.particles.pop(0)
|
|
return p
|
|
|
|
def destroy(self):
|
|
self.world.DestroyBody(self.hull)
|
|
self.hull = None
|
|
for w in self.wheels:
|
|
self.world.DestroyBody(w)
|
|
self.wheels = []
|