def tracking(args):
track_layout = args["track_layout"]
track_spec = np.genfromtxt("data/track_layout/" + track_layout + ".csv",
delimiter=",")
if args["simulation"]:
track = racing_env.ClosedTrack(track_spec, track_width=0.8)
# setup ego car
ego = offboard.DynamicBicycleModel(
name="ego",
param=base.CarParam(edgecolor="black"),
system_param=base.SystemParam())
ego.set_state_curvilinear(np.zeros((X_DIM, )))
ego.set_state_global(np.zeros((X_DIM, )))
ego.start_logging()
if args["ctrl_policy"] == "pid":
ego.set_ctrl_policy(offboard.PIDTracking(vt=0.8))
elif args["ctrl_policy"] == "mpc-lti":
mpc_lti_param = base.MPCTrackingParam(vt=0.8)
ego.set_ctrl_policy(
offboard.MPCTracking(mpc_lti_param, ego.system_param))
else:
raise NotImplementedError
ego.ctrl_policy.set_timestep(0.1)
ego.ctrl_policy.set_track(track)
ego.set_track(track)
# setup simulation
simulator = offboard.CarRacingSim()
simulator.set_timestep(0.1)
simulator.set_track(track)
simulator.add_vehicle(ego)
ego.ctrl_policy.set_racing_sim(simulator)
simulator.sim(sim_time=90.0)
with open("data/simulator/tracking.obj", "wb") as handle:
pickle.dump(simulator, handle, protocol=pickle.HIGHEST_PROTOCOL)
else:
with open("data/simulator/tracking.obj", "rb") as handle:
simulator = pickle.load(handle)
if args["plotting"]:
simulator.plot_simulation()
simulator.plot_state("ego")
if args["animation"]:
simulator.animate(filename="tracking", ani_time=250)
def set_up_ego(timestep, track):
ego = offboard.DynamicBicycleModel(name="ego",
param=base.CarParam(edgecolor="black"),
system_param=base.SystemParam())
ego.set_timestep(timestep)
# run the pid controller for the first lap to collect data
pid_controller = offboard.PIDTracking(vt=0.7, eyt=0.0)
pid_controller.set_timestep(timestep)
ego.set_ctrl_policy(pid_controller)
pid_controller.set_track(track)
ego.set_state_curvilinear(np.zeros((X_DIM, )))
ego.set_state_global(np.zeros((X_DIM, )))
ego.start_logging()
ego.set_track(track)
# run mpc-lti controller for the second lap to collect data
mpc_lti_param = base.MPCTrackingParam(vt=0.7, eyt=0.0)
mpc_lti_controller = offboard.MPCTracking(mpc_lti_param, ego.system_param)
mpc_lti_controller.set_timestep(timestep)
mpc_lti_controller.set_track(track)
return ego, pid_controller, mpc_lti_controller
def linear_time_invariant():
# define closed_track
track_spec = np.array([
[3, 0],
[np.pi / 2 * 1.5, -1.5],
[2, 0],
[np.pi / 2 * 1.5, -1.5],
[6, 0],
[np.pi / 2 * 1.5, -1.5],
[2.0, 0],
[np.pi / 2 * 1.5, -1.5],
])
track = racing_env.ClosedTrack(track_spec, track_width=1.0)
# setup ego car
ego = offboard.DynamicBicycleModel(name="ego",
param=base.CarParam(edgecolor="black"))
ego.set_state_curvilinear(np.array([0.3, 0, 0, 0, 0, 0]))
ego.set_state_global(np.array([0.3, 0, 0, 0, 0, 0]))
ego.set_ctrl_policy(offboard.PIDTracking(vt=0.5))
ego.ctrl_policy.set_timestep(0.1)
ego.set_track(track)
# setup simulation
simulator = offboard.CarRacingSim()
simulator.set_timestep(0.1)
simulator.set_track(track)
simulator.add_vehicle(ego)
ego.ctrl_policy.set_racing_sim(simulator)
simulator.sim(sim_time=500.0)
# calculate linearized dynamics
xdata = np.stack(simulator.vehicles["ego"].xcurv_log, axis=0)
udata = system_identification.get_udata(simulator.vehicles["ego"])
lamb = 1e-9
matrix_A, matrix_B, error = system_identification.linear_regression(
xdata, udata, lamb)
np.savetxt("data/track_layout/ellipse.csv", track_spec, delimiter=",")
np.savetxt("data/sys/LTI/matrix_A.csv", matrix_A, delimiter=",")
np.savetxt("data/sys/LTI/matrix_B.csv", matrix_B, delimiter=",")
print(matrix_A)
print(matrix_B)