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")