def update(iter):
global lambda_, xi
t = DT * iter
tau = calc_e_stabilize_law(xi, lambda_, x0)
xi = update_state(lambda t, xi: state_equation(xi, tau), (t, t + DT), xi)
lambda_ = update_lambda(
lambda t, l_: calc_lambda_dot(l_, clambda, epsilon), (t, t + DT),
lambda_)
(
car_x,
car_y,
car_angle_x,
car_angle_y,
left_tire_x,
left_tire_y,
right_tire_x,
right_tire_y,
) = plot_car(xi[0:3])
plots["car"].set_data(car_x, car_y)
plots["car_angle"].set_data(car_angle_x, car_angle_y)
plots["lefT_tire"].set_data(left_tire_x, left_tire_y)
plots["right_tire"].set_data(right_tire_x, right_tire_y)
plots["tau0"].set_offsets((t, tau[0]))
plots["tau1"].set_offsets((t, tau[1]))
def calc_e_stabilize_law(xi, lambda_, x0):
x = xi[0:2]
theta = xi[2]
rot_l_inv = np.empty((2, 2))
rot_l_inv[0, 0] = cos(theta)
rot_l_inv[0, 1] = sin(theta)
rot_l_inv[1, 0] = -sin(theta) / lambda_
rot_l_inv[1, 1] = cos(theta) / lambda_
q = x + lambda_ * np.array([cos(theta), sin(theta)])
lambda_dot = calc_lambda_dot(lambda_, clambda, epsilon)
e1 = np.zeros(2)
e1[0] = 1
tau = -cp * rot_l_inv @ (q - x0) - lambda_dot * e1
return tau
def calc_e_stabilize_law(xi, lambda_, x0):
x = xi[0:2]
v = xi[3:5]
w = xi[4]
theta = xi[2]
rot_l_inv = np.empty((2, 2))
rot_l_inv[0, 0] = cos(theta)
rot_l_inv[0, 1] = sin(theta)
rot_l_inv[1, 0] = -sin(theta) / lambda_
rot_l_inv[1, 1] = cos(theta) / lambda_
q = x + lambda_ * np.array([cos(theta), sin(theta)])
lambda_dot = calc_lambda_dot(lambda_, clambda, epsilon)
e1 = np.zeros(2)
e1[0] = 1
whl = calc_w_hat_lambda(w, lambda_, lambda_dot)
tau = (-cp * rot_l_inv @ (q - x0) - cd * v - cd * lambda_dot * e1 -
whl @ v - lambda_dot * (whl - clambda * np.identity(2)) @ e1)
return tau
xi = initialize_xi()
lambda_ = LAMBDA_INIT
x0 = np.zeros(2)
N = len(np.arange(0, TF, DT))
xi_hist = np.empty((5, N))
tau_hist = np.empty((2, N))
t_hist = np.empty(N)
# save trajectory
for (step, t) in enumerate(np.arange(0, TF, DT)):
tau = calc_e_stabilize_law(xi, lambda_, x0)
xi = update_state(lambda t, xi: state_equation(xi, tau), (t, t + DT), xi)
lambda_ = update_lambda(
lambda t, l_: calc_lambda_dot(l_, clambda, epsilon), (t, t + DT),
lambda_)
xi_hist[:, step] = xi
tau_hist[:, step] = tau
t_hist[step] = t
# reset for animation
xi = initialize_xi()
lambda_ = LAMBDA_INIT
fig, axes = plt.subplots(1, 2)
ax1, ax2 = axes
ax1.set_xlim(0, 3.25)
ax1.set_ylim(0, 3.25)
ax1.set_aspect("equal")