-
Notifications
You must be signed in to change notification settings - Fork 0
/
Car.py
47 lines (36 loc) · 1.38 KB
/
Car.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
import numpy as np
class Car:
def __init__(self, x=0, y=0, psi=0, v=0, delta=0, L=2.5):
# The configuration of Car
self.x = x # x position
self.y = y # y position
self.psi = psi # yaw angle
self.v = v # velocity
self.delta = delta # steering angle
self.a = 0 # acceleration
self.omega = 0 # steering angle rate
self.L = L # Wheelbase
# The paths
self.path = [np.array([self.x, self.y, self.psi, self.v, self.delta, self.a, self.omega])]
# The constraints of the car
self.max_v = 1.5
self.min_v = -self.max_v
self.max_delta = np.pi / 6
self.min_delta = -self.max_delta
self.max_a = 1.5
self.min_a = -self.max_a
self.max_omega = 0.5
self.min_omega = -self.max_omega
self.d = 0.3
def update_state(self, a, omega, dt):
# The dynamic equations
self.x += self.v * np.cos(self.psi) * dt
self.y += self.v * np.sin(self.psi) * dt
self.psi += self.v * np.tan(self.delta) / self.L * dt
self.v += a * dt
self.delta += omega * dt
self.a = a
self.omega = omega
# Add current configuration to paths
self.path.append(np.array([self.x, self.y, self.psi, self.v, self.delta, self.a, self.omega]))
return self.x, self.y, self.psi, self.v, self.delta