Class 實作:
import numpy as np
class Bicycle():
def __init__(self):
self.xc = 0
self.yc = 0
self.theta = 0
self.delta = 0
self.beta = 0
self.L = 2
self.lr = 1.2
self.w_max = 1.22
self.sample_time = 0.01
def reset(self):
self.xc = 0
self.yc = 0
self.theta = 0
self.delta = 0
self.beta = 0
def step(self, v, w):
# ==================================
# Implement kinematic model here
# ==================================
if w > 0:
w = min(w, self.w_max)
else:
w = max(w, -self.w_max)
self.delta += (w * self.sample_time)
self.beta = np.arctan(self.lr * np.tan(self.delta) / self.L)
theta_dot = v * np.cos(self.beta) * np.tan(self.delta) / self.L
self.theta += theta_dot * self.sample_time
self.xc += (v * np.cos(self.theta + self.beta)) * self.sample_time
self.yc += (v * np.sin(self.theta + self.beta)) * self.sample_time
要怎麼畫出 8 - Concept:
要怎麼畫出 8 - 實作
sample_time = 0.01
time_end = 30
model.reset()
t_data = np.arange(0,time_end,sample_time)
x_data = np.zeros_like(t_data)
y_data = np.zeros_like(t_data)
v_data = np.zeros_like(t_data)
w_data = np.zeros_like(t_data)
# ==================================
# Learner solution begins here
# ==================================
radius = 8
v_data = np.zeros_like(t_data)
v_data[:] = 2 * (2 * radius * np.pi) / time_end
w_data = np.zeros_like(t_data)
delta = 0.985 * np.arctan(model.L/radius)
for i in range(t_data.shape[0]):
x_data[i] = model.xc
y_data[i] = model.yc
if i <= t_data.shape[0]/8:
if model.delta < delta:
model.step(v_data[i], model.w_max)
w_data[i] = model.w_max
else:
model.step(v_data[i], 0)
w_data[i] = 0
elif i <= 5.05 * t_data.shape[0]/8:
if model.delta > -delta:
model.step(v_data[i], -model.w_max)
w_data[i] = -model.w_max
else:
model.step(v_data[i], 0)
w_data[i] = 0
else:
if model.delta < delta:
model.step(v_data[i], model.w_max)
w_data[i] = model.w_max
else:
model.step(v_data[i], 0)
w_data[i] = 0
# print("step=%i, (x=%s, y=%s), (model=%s, delta=%s)" % (i, np.round(x_data[i], 4), np.round(y_data[i], 4), np.round(model.delta, 6), np.round(delta, 6)))
# ==================================
# Learner solution ends here
# ==================================
plt.axis('equal')
plt.plot(x_data, y_data)
plt.show()