if not TRACK_CONS:
SUFFIX = 'NOCONS-'
else:
SUFFIX = ''
#####################################################################
# load vehicle parameters
params = ORCA(control='pwm')
model = Dynamic(**params)
#####################################################################
# load track
TRACK_NAME = 'ETHZ'
track = ETHZ(reference='optimal', longer=True)
SIM_TIME = 8.5
#####################################################################
# load GP models
with open('../gp/orca/vxgp.pickle', 'rb') as f:
(vxmodel, vxxscaler, vxyscaler) = pickle.load(f)
vxgp = loadGPModel('vx', vxmodel, vxxscaler, vxyscaler)
with open('../gp/orca/vygp.pickle', 'rb') as f:
(vymodel, vyxscaler, vyyscaler) = pickle.load(f)
vygp = loadGPModel('vy', vymodel, vyxscaler, vyyscaler)
with open('../gp/orca/omegagp.pickle', 'rb') as f:
(omegamodel, omegaxscaler, omegayscaler) = pickle.load(f)
omegagp = loadGPModel('omega', omegamodel, omegaxscaler, omegayscaler)
gpmodels = {
SAVE_RESULTS = True # whether to save results
N_WAYPOINTS = 100 # resampled waypoints
SCALE = 0.95 # shrinking factor for track width
# define indices for the nodes
NODES = [
5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95,
105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, 170, 175,
180, 185
]
#####################################################################
# track specific data
params = ORCA()
track = ETHZ()
track_width = track.track_width * SCALE
theta = track.theta_track[NODES]
N_DIMS = len(NODES)
n_waypoints = N_DIMS
rand_traj = randomTrajectory(track=track, n_waypoints=n_waypoints)
bounds = torch.tensor(
[[-track_width / 2] * N_DIMS, [track_width / 2] * N_DIMS],
device=device,
dtype=dtype)
def evaluate_y(x_eval, mean_y=None, std_y=None):
filepath = 'track_validation.png'
label = 'MPC (exact)'
N_SAMPLES = 400
THRESHOLD = 0.25
#####################################################################
# load vehicle parameters
params = ORCA(control='pwm')
model = Dynamic(**params)
#####################################################################
# load track
if TRACK_NAME == 'ETHZ':
track = ETHZ(reference='optimal') # ETHZ() or ETHZMobil()
elif TRACK_NAME == 'ETHZMobil':
track = ETHZMobil(reference='optimal') # ETHZ() or ETHZMobil()
#####################################################################
# load inputs used to simulate Dynamic model
data = np.load('../data/DYN-{}-{}.npz'.format(CTYPE, TRACK_NAME))
time = data['time'][:N_SAMPLES + 1]
states = np.zeros([7, N_SAMPLES + 1])
states[:6, :] = data['states'][:6, :N_SAMPLES + 1]
states[6, 1:] = data['inputs'][1, :N_SAMPLES]
inputs = data['inputs'][:, :N_SAMPLES]
inputs[1, 0] = inputs[1, 0]
x.append(ix)
y.append(iy)
return x, y
if __name__ == '__main__':
""" example how to use
"""
# choose track 'ETHZ'/'ETHZMobil'/'UCB'
TRACK_NAME = 'UCB'
# choose vehicle params and specify indices of the nodes
if TRACK_NAME is 'ETHZ':
params = ORCA()
track = ETHZ()
NODES = [
33, 67, 116, 166, 203, 239, 274, 309, 344, 362, 382, 407, 434, 448,
470, 514, 550, 586, 622, 657, 665
]
elif TRACK_NAME is 'ETHZMobil':
params = ORCA()
track = ETHZMobil()
NODES = [
7, 21, 37, 52, 66, 81, 97, 111, 136, 160, 175, 191, 205, 220, 236,
250, 275, 299, 337, 376
]
elif TRACK_NAME is 'UCB':
params = F110()
SIM_TIME = 10 # in [s]
LD = 0.1
KP = 2
#####################################################################
# load vehicle parameters
params = ORCA(control='pwm')
model = Dynamic(**params)
#####################################################################
# load track
TRACK_NAME = 'ETHZMobil'
if TRACK_NAME == 'ETHZ':
track = ETHZ(reference='optimal') # ETHZ() or ETHZMobil()
elif TRACK_NAME == 'ETHZMobil':
track = ETHZMobil(reference='optimal') # ETHZ() or ETHZMobil()
#####################################################################
# extract data
Ts = SAMPLING_TIME
n_steps = int(SIM_TIME / Ts)
n_states = model.n_states
n_inputs = model.n_inputs
#####################################################################
# closed-loop simulation
# initialize
#####################################################################
# settings
SAVE_RESULTS = False
SAMPLING_TIME = 0.02
HORIZON = 20
#####################################################################
# load track
N_SAMPLES = 390
TRACK_NAME = 'ETHZ'
LAPS_DONE = 1
track = ETHZ(reference='optimal', longer=True)
#####################################################################
# load inputs used to simulate Dynamic model
SUFFIX = 'NOCONS-'
data = np.load('../data/DYN-NMPC-{}{}.npz'.format(SUFFIX, TRACK_NAME))
time_dyn = data['time'][:N_SAMPLES+1]
states_dyn = data['states'][:,:N_SAMPLES+1]
inputs_dyn = data['inputs'][:,:N_SAMPLES]
data = np.load('../data/DYN-GPMPC-{}{}.npz'.format(SUFFIX, TRACK_NAME))
time_gp = data['time'][:N_SAMPLES+1]
states_gp = data['states'][:,:N_SAMPLES+1]
inputs_gp = data['inputs'][:,:N_SAMPLES]