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):
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 = {
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]