-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathslip_test.py
119 lines (105 loc) · 4.02 KB
/
slip_test.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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
# -*- coding: utf-8 -*-
"""
Created on Mon Apr 27 10:09:15 2020
@author: scheu
"""
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
class CarState:
x = 0
y = 0
theta = 0
velocity = 0
steer_angle = 0
angular_velocity = 0
slip_angle = 0
st_dyn = True
class CarParam:
wheelbase = 0.3302
friction_coeff = 0.523
h_cg = 0.074
l_f = 0.17145
l_r = 0.15875
cs_f = 4.718
cs_r =5.4562
mass = 3.47
I_z = 0.04712
def update(start: CarState, accel, steer_angle_vel, p: CarParam, dt):
end = CarState()
g = 9.81
x_dot = start.velocity * np.cos(start.theta + start.slip_angle)
y_dot = start.velocity * np.sin(start.theta + start.slip_angle)
v_dot = accel
steer_angle_dot = steer_angle_vel
theta_dot = start.angular_velocity
rear_val = g * p.l_r - accel * p.h_cg
front_val = g * p.l_f + accel * p.h_cg
if (start.velocity == 0):
vel_ratio = 0
first_term = 0
else:
vel_ratio = start.angular_velocity / start.velocity
first_term = p.friction_coeff / (start.velocity * (p.l_r + p.l_f))
theta_double_dot = (p.friction_coeff * p.mass / (p.I_z * p.wheelbase)) * (p.l_f * p.cs_f * start.steer_angle * (rear_val) + start.slip_angle * (p.l_r * p.cs_r * (front_val) - p.l_f * p.cs_f * (rear_val)) - vel_ratio * ((p.l_f**2) * p.cs_f * (rear_val) + (p.l_r**2) * p.cs_r * (front_val)))
slip_angle_dot = (first_term) * (p.cs_f * start.steer_angle * (rear_val) - start.slip_angle * (p.cs_r * (front_val) + p.cs_f * (rear_val)) + vel_ratio * (p.cs_r * p.l_r * (front_val) - p.cs_f * p.l_f * (rear_val))) - start.angular_velocity
end.x = start.x + x_dot * dt
end.y = start.y + y_dot * dt
end.theta = start.theta + theta_dot * dt
end.velocity = start.velocity + v_dot * dt
end.steer_angle = start.steer_angle + steer_angle_dot * dt
end.angular_velocity = start.angular_velocity + theta_double_dot * dt
end.slip_angle = start.slip_angle + slip_angle_dot * dt
end.st_dyn = True
return end
def get_slip(velocity, steering_angle, eps):
initial = CarState()
initial.velocity = velocity
initial.steer_angle = steering_angle
new = initial
params = CarParam()
diff = 10
old_slip = 10
while diff > eps:
new = update(new, 0, 0, params, 0.01)
diff = np.abs(new.slip_angle - old_slip)
old_slip = new.slip_angle
return old_slip
if __name__ == "__main__":
desired_slip = 0.3
velocities = np.arange(0.5, 4.6, 0.1)
steering_angles = np.arange(0.01, 0.4189, 0.01)
vel_plot = np.array([])
steer_plot = np.array([])
slip_angles = np.array([])
vel_plot_3, steer_plot_3 = np.meshgrid(velocities, steering_angles)
#steer_plot_3 = np.array([])
slip_angles_3 = np.zeros(vel_plot_3.shape)
for ii, vel in enumerate(velocities):
for jj, ang in enumerate(steering_angles):
slip = get_slip(vel, ang, 0.0000000001)
#vel_plot_3 = np.append(vel_plot_3, vel)
#steer_plot_3 = np.append(steer_plot_3, ang)
#slip_angles_3 = np.append(slip_angles_3, slip)
slip_angles_3[ii, jj] = slip
if (abs(slip + desired_slip) < 0.01):
vel_plot = np.append(vel_plot, vel)
steer_plot = np.append(steer_plot, ang)
slip_angles = np.append(slip_angles, slip)
fig = plt.figure()
ax = fig.add_subplot(111)
ax.plot(vel_plot, steer_plot, 'b.')
ax.plot([3.6, 4.5], [0.40, 0.23], 'r')
ax.set_xlabel('Velocity')
ax.set_ylabel('Steering Angle')
## 3d
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot_surface(vel_plot_3, steer_plot_3, slip_angles_3, cmap=cm.coolwarm)
ax.set_xlabel('Velocity')
ax.set_ylabel('Steering Angle')
ax.set_zlabel('Slip Angle')
plt.show()
ax.set_zlim([-0.5,0])
plt.show()