def __init__(self):
self.p_low = np.array([-0.5, -.5, 0.5])
self.p_high = np.array([+0.5, +0.5, 0.8])
self.duration_low = 1.
self.duration_high = 8.
self.n_waypoints = 100
self.train_nominal_model = False
# Initialize robot
bintel = Robot()
go_waypoint = MavrosGOTOWaypoint()
print("Moving to initial point...")
p_init = np.array([0., 0., 1.])
p_final = np.array([0., 0., 1.])
go_waypoint.gopoint(np.array(p_init))
for experiment in range(self.n_waypoints):
p_init = np.copy(p_final)
x = self.p_low[0] + (self.p_high[0] -
self.p_low[0]) * np.random.rand()
y = self.p_low[1] + (self.p_high[1] -
self.p_low[1]) * np.random.rand()
z = self.p_low[2] + (self.p_high[2] -
self.p_low[2]) * np.random.rand()
t = self.duration_low + (self.duration_high -
self.duration_low) * np.random.rand()
if self.train_nominal_model:
x = 0.
y = 0.
p_final = np.array([x, y, z])
print("Waypoint ", experiment, ": ", p_final, ", duration: ", t)
bintel.gotopoint(p_init, p_final, t)
print("Experiments finalized, moving to initial point...")
go_waypoint.gopoint(np.array([0., 0., 0.5]))
print("Landing")
land()
def __init__(self):
self.duration_low = 1.
self.n_waypoints = 1
self.controller_rate = 60
# Initialize robot
bintel = Robot(self.controller_rate,3,1)
go_waypoint = MavrosGOTOWaypoint()
print("Moving to initial point...")
p_init = np.array([1., -1.5, 1.5])
p_final = np.array([1, -1.5, 0.5])
go_waypoint.gopoint(np.array(p_init))
for experiment in range(self.n_waypoints):
print("Waypoint ", experiment, ": ", p_final, ", duration: ", self.duration_low)
bintel.gotopoint(p_init, p_final, self.duration_low)
print("Experiments finalized, moving to initial point...")
#go_waypoint.gopoint(np.array([0., 0., 0.5]))
print("Landing")
land()
def __init__(self):
# Read mission file
config_file = 'scripts/mission.yaml'
mission = self.read_mission(config_file)
mission_folder = 'data/dataexp' + datetime.now().strftime(
"%Y-%m-%d_%H-%M-%S")
os.mkdir(mission_folder)
shutil.copy(config_file, mission_folder)
self.p_init = mission['trajectory']['points'][0]
self.p_final = mission['trajectory']['points'][1]
self.duration = mission['trajectory']['duration']
# Initialize robot
bintel = Robot()
#bintel.plot_desired_traj(self.p_init, self.p_final, self.duration)
go_waypoint = MavrosGOTOWaypoint()
print("Moving to initial point...{}".format(self.p_init))
go_waypoint.gopoint(self.p_init)
for experiment in range(mission['trajectory']['iterations']):
self.csv_file = mission_folder + '/csv_data_' + str(
experiment) + '.csv'
self.file = open(self.csv_file, 'w')
print("Launching position controller...")
bintel.gotopoint(self.p_init, self.p_final, self.duration,
self.file)
self.file.close()
go_waypoint.gopoint(self.p_init)
print("Experiments finalized, landing")
land()
plot_error(mission_folder, self.duration)
p_final,
duration_low,
controller=handler)
#land()
X, Xd, U, Unom, t, _ = handler.clean_data(X, p_final, U, Upert, t)
# Must locally aggregate data from each waypoint and feed aggregated matrices to fit_diffeomorphism
X_w.append(X)
Xd_w.append(Xd)
U_w.append(U)
Unom_w.append(Unom)
t_w.append(t)
#osqp_thoughts[ww][ep] = bintel.osqp_thoughts
land() # Land while fitting models
X, Xd, U, Unom, t = handler.aggregate_landings_per_episode(
X_w, Xd_w, U_w, Unom_w, t_w)
print("Fitting diffeomorphism...")
eigenfunction_basis.fit_diffeomorphism_model(
X=array(X.transpose()),
t=t.transpose(),
X_d=array(Xd.transpose()),
l2=l2_diffeomorphism,
learning_rate=diff_learn_rate,
learning_decay=diff_learn_rate_decay,
n_epochs=diff_n_epochs,
train_frac=diff_train_frac,
batch_size=diff_batch_size,
initialize=initialize_NN,
verbose=True)