def goto_starting_pos(self):
p = Position()
p.x = int(0)
p.y = int(0)
p.z = int(-50)
p.speed = int(1400)
self.create_simulated_data(p, self.prev_pos)
self.prev_pos = p
def run_babbling(self):
p = Position()
for _ in range(self.max_iterations):
#test_motor_pred = self.inverse_model.predict([self.test_images[0:self.goal_size*self.goal_size]])
#print np.hstack((test_motor_pred,self.test_pos[0:self.goal_size*self.goal_size]))
# record logs and data
self.get_current_inv_mse()
self.get_current_fwd_mse()
#self.log_lr_inv.append(K.eval(self.inverse_model.optimizer.lr))
#print 'iteration opt inv', K.eval(self.inverse_model.optimizer.iteration)
#print 'current lr: ', self.log_lr_inv[-1]
print('Mode ', self.goal_selection_mode, ' hist_size ',
str(self.history_size), ' prob ',
str(self.history_buffer_update_prob), ' iteration : ',
self.iteration)
self.iteration = self.iteration + 1
# if self.iteration > self.max_iterations:
# self.save_models()
# return
# select a goal
self.current_goal_idx, self.current_goal_x, self.current_goal_y = self.interest_model.select_goal(
)
if self.goal_selection_mode == 'kmeans':
self.goal_code = self.kmeans.cluster_centers_[
self.current_goal_idx].reshape(1, self.code_size)
print(' code ', self.goal_code)
elif self.goal_selection_mode == 'db' or self.goal_selection_mode == 'random':
#self.goal_image=np.asarray(cv2.imread(self.goal_db[self.current_goal_idx], 0)).reshape(1, self.image_size, self.image_size, self.channels).astype('float32') / 255
self.goal_image = self.test_images[
self.current_goal_idx].reshape(1, self.image_size,
self.image_size,
self.channels)
self.goal_code = self.encoder.predict(self.goal_image)
elif self.goal_selection_mode == 'som':
self.goal_code = self.goal_som._weights[
self.current_goal_x,
self.current_goal_y].reshape(1, self.code_size)
else:
print('wrong goal selection mode, exit!')
sys.exit(1)
motor_pred = []
if self.goal_selection_mode == 'db':
#cv2.imshow("Goal", cv2.imread(self.goal_db[self.current_goal_idx], 0))
####cv2.imshow("Goal", self.test_images[self.current_goal_idx])
####cv2.waitKey(1)
#motor_pred = self.inverse_model.predict(self.goal_image)
motor_pred = self.inverse_code_model.predict(self.goal_code)
print('pred ', motor_pred, ' real ',
self.test_pos[self.current_goal_idx])
else:
goal_decoded = self.decoder.predict(self.goal_code)
####cv2.imshow("CAE Decoded Goal", goal_decoded[0])
####cv2.waitKey(1)
# motor_pred = self.inverse_model.predict(goal_decoded)
motor_pred = self.inverse_code_model.predict(self.goal_code)
#image_pred = self.forward_model.predict(np.asarray(motor_pred))
image_pred = self.decoder.predict(
self.forward_code_model.predict(np.asarray(motor_pred)))
####cv2.imshow("FWD Model Predicted Image", image_pred[0])
####cv2.waitKey(1)
#image_pred_curr = forward_model.predict(np.asarray(prev_cmd))
#cv2.imshow("FWD Model Predicted Image curr ", image_pred_curr[0])
#cv2.waitKey(1)
# motor_cmd=motor_pred[0]
# p.x = clamp_x(motor_pred[0][0]*x_lims[1])
# p.y = clamp_x(motor_pred[0][1]*y_lims[1])
noise_x = np.random.normal(0, 0.02)
noise_y = np.random.normal(0, 0.02)
p.x = clamp_x((motor_pred[0][0] + noise_x) * x_lims[1])
p.y = clamp_y((motor_pred[0][1] + noise_y) * y_lims[1])
ran = random.random()
if ran < self.random_cmd_rate or (
len(self.history_pos) / self.batch_size
) != self.history_size or self.goal_selection_mode == 'random': # choose random motor commands from time to time
print('generating random motor command')
p.x = random.uniform(x_lims[0], x_lims[1])
p.y = random.uniform(y_lims[0], y_lims[1])
self.random_cmd_flag = True
else:
self.random_cmd_flag = False
print('predicted position ', motor_pred[0], 'p+noise ',
motor_pred[0][0] + noise_x, ' ', motor_pred[0][1] + noise_y,
' clamped ', p.x, ' ', p.y, ' noise.x ', noise_x, ' n.y ',
noise_y)
p.z = int(-90)
p.speed = int(1400)
self.create_simulated_data(p, self.prev_pos)
self.prev_pos = p
if self.iteration % 50 == 0:
#test_codes= self.encoder.predict(self.test_images[0:self.goal_size*self.goal_size].reshape(self.goal_size*self.goal_size, self.image_size, self.image_size, self.channels))
if self.goal_selection_mode == 'db' or self.goal_selection_mode == 'random':
plot_cvh(self.convex_hull_inv,
title=self.goal_selection_mode + '_' +
str(self.exp_iteration) + 'cvh_inv',
iteration=self.iteration,
dimensions=2,
log_goal=self.test_pos[0:self.goal_size *
self.goal_size],
num_goals=self.goal_size * self.goal_size)
elif self.goal_selection_mode == 'kmeans':
goals_pos = self.inverse_code_model.predict(
self.kmeans.cluster_centers_[0:self.goal_size *
self.goal_size].reshape(
self.goal_size *
self.goal_size,
self.code_size))
plot_cvh(self.convex_hull_inv,
title=self.goal_selection_mode + '_' +
str(self.exp_iteration) + 'cvh_inv',
iteration=self.iteration,
dimensions=2,
log_goal=goals_pos,
num_goals=self.goal_size * self.goal_size)
elif self.goal_selection_mode == 'som':
goals_pos = self.inverse_code_model.predict(
self.goal_som._weights.reshape(
len(self.goal_som._weights) *
len(self.goal_som._weights[0]),
len(self.goal_som._weights[0][0])))
plot_cvh(self.convex_hull_inv,
title=self.goal_selection_mode + '_' +
str(self.exp_iteration) + 'cvh_inv',
iteration=self.iteration,
dimensions=2,
log_goal=goals_pos,
num_goals=self.goal_size * self.goal_size)
#test_codes_ipca = self.fwd_ipca.fit_transform(test_codes)
#plot_cvh(self.convex_hull_fwd, title=self.goal_selection_mode+'_'+str(self.exp_iteration)+'cvh_fwd', iteration = self.iteration, dimensions=2, log_goal=test_codes_ipca, num_goals=self.goal_size*self.goal_size)
observation = self.img[-1]
####cv2.imshow("Current observation", observation)
# update competence of the current goal (it is supposed that at this moment the action is finished
if len(self.img) > 0 and (not self.random_cmd_flag
or self.goal_selection_mode == 'random'):
#observation_code = self.encoder.predict(observation.reshape(1, self.image_size, self.image_size, self.channels))
#prediction_error = np.linalg.norm(np.asarray(self.goal_code[:])-np.asarray(observation_code[:]))
cmd = [p.x / float(x_lims[1]), p.y / float(y_lims[1])]
prediction_code = self.forward_code_model.predict(
np.asarray(cmd).reshape((1, 2)))
prediction_error = np.linalg.norm(
np.asarray(self.goal_code[:]) -
np.asarray(prediction_code[:]))
self.interest_model.update_competences(self.current_goal_x,
self.current_goal_y,
prediction_error)
#print 'Prediction error: ', prediction_error, ' learning progress: ', self.interest_model.get_learning_progress(self.current_goal_x, self.current_goal_y)
self.log_lp.append(
np.asarray(deepcopy(
self.interest_model.learning_progress)))
self.log_goal_id.append(self.current_goal_idx)
#print self.log_lp
# fit models
if len(self.img) > self.batch_size:
if len(self.img) == len(self.pos):
# first fill the history buffer, then update the models
if (len(self.history_pos) /
self.batch_size) != self.history_size:
if len(self.history_pos) == 0:
self.history_pos = deepcopy(
self.pos[-(self.batch_size):])
self.history_img = deepcopy(
self.img[-(self.batch_size):])
self.history_pos = np.vstack(
(self.history_pos, self.pos[-(self.batch_size):]))
self.history_img = np.vstack(
(self.history_img, self.img[-(self.batch_size):]))
else:
img_io = []
pos_io = []
if (self.history_size != 0):
for i in range(-(self.batch_size), 0):
#print i
r = random.random()
if r < self.history_buffer_update_prob:
r_i = random.randint(
0,
self.history_size * self.batch_size -
1)
#print 'r_i ', r_i, ' i ', i
self.history_pos[r_i] = deepcopy(
self.pos[i])
self.history_img[r_i] = deepcopy(
self.img[i])
# update models
img_io = np.vstack(
(np.asarray(self.history_img[:]).reshape(
len(self.history_img), self.image_size,
self.image_size, self.channels),
np.asarray(
self.img[-(self.batch_size):]).reshape(
self.batch_size, self.image_size,
self.image_size, self.channels)))
pos_io = np.vstack(
(np.asarray(self.history_pos[:]),
np.asarray(self.pos[-(self.batch_size):])))
else:
img_io = np.asarray(
self.img[-(self.batch_size):]).reshape(
self.batch_size, self.image_size,
self.image_size, self.channels)
pos_io = np.asarray(self.pos[-(self.batch_size):])
#models.train_forward_model_on_batch(self.forward_model, pos_io, img_io, batch_size=self.batch_size, epochs = self.epochs)
codes_io = self.encoder.predict(img_io)
models.train_forward_code_model_on_batch(
self.forward_code_model,
pos_io,
codes_io,
batch_size=self.batch_size,
epochs=self.fwd_epochs)
#models.train_inverse_model_on_batch(self.inverse_model, img_io, pos_io, batch_size=self.batch_size, epochs = self.inv_epochs)
models.train_inverse_code_model_on_batch(
self.inverse_code_model,
codes_io,
pos_io,
batch_size=self.batch_size,
epochs=self.inv_epochs)
#train_autoencoder_on_batch(self.autoencoder, self.encoder, self.decoder, np.asarray(self.img[-32:]).reshape(32, self.image_size, self.image_size, self.channels), batch_size=self.batch_size, cae_epochs=5)
# update convex hulls
obs_codes = self.encoder.predict(
np.asarray(self.img[-(self.batch_size):]).reshape(
self.batch_size, self.image_size, self.image_size,
self.channels))
#print self.pos[-32:]
#print np.asarray(self.pos[-32:]).reshape((32,2))
self.convex_hull_inv.add_points(np.asarray(
self.pos[-(self.batch_size):]).reshape(
((self.batch_size), 2)),
restart=True)
self.log_cvh_inv.append(self.convex_hull_inv.volume)
#print 'conv hull inv. volume: ', self.convex_hull_inv.volume
#print 'a', obs_codes[:]
#self.fwd_ipca.partial_fit(obs_codes[:])
#ipca_codes = self.fwd_ipca.transform(obs_codes[:])
#print 'p', ipca_codes
#print 'p', ipca_codes[:]
#self.convex_hull_fwd.add_points(np.asarray(obs_codes[:]), restart=False)
# this may be inconsistent. IPCA could change over time, so previous points projected for the CVH calculation could change. Check this
#self.convex_hull_fwd.add_points(np.asarray(ipca_codes[:]), restart=True)
#print 'conv hull fwd. volume: ', self.convex_hull_fwd.volume
#self.log_cvh_fwd.append(self.convex_hull_fwd.volume)
else:
print('lenghts not equal')
if not self.random_cmd_flag and len(self.cmd) > 0:
#print 'test pos', self.test_positions[0:10]
test_p = self.test_pos[self.current_goal_idx]
#curr_cmd = self.cmd[-1]
#pred_p = self.inverse_model.predict(self.goal_image)
pred_p = self.inverse_code_model.predict(self.goal_code)
self.log_goal_pos[self.current_goal_idx].append(
[test_p[0], test_p[1]])
#self.log_curr_pos[self.current_goal_idx].append([curr_cmd[0], curr_cmd[1] ])
self.log_goal_pred[self.current_goal_idx].append(
[pred_p[0][0], pred_p[0][1]])
#print 'hg ', self.log_goal_pos
#g_code = self.forward_model.predict(np.asarray(test_p).reshape((1,2)))
#g_code = self.decoder.predict(self.forward_code_model.predict(np.asarray(test_p).reshape((1,2))))
#g_code = self.fwd_ipca.transform(self.forward_code_model.predict(np.asarray(test_p).reshape((1,2))) )
#c_code = self.forward_model.predict(np.asarray(curr_cmd).reshape((1,2)))
#c_code = self.decoder.predict(self.forward_code_model.predict(np.asarray(curr_cmd).reshape((1,2))) )
#c_code = self.fwd_ipca.transform(self.forward_code_model.predict(np.asarray(curr_cmd).reshape((1,2))) )
#print 'g ipca ', g_code
#print 'c ipca ', c_code
#self.log_goal_img[self.current_goal_idx].append([g_code[0][0],g_code[0][1], g_code[0][2],g_code[0][3] ])
#self.log_curr_img[self.current_goal_idx].append([c_code[0][0],c_code[0][1], c_code[0][2],c_code[0][3] ])
#self.log_goal_img[self.current_goal_idx].append([g_code[0][0],g_code[0][1] ])
#self.log_curr_img[self.current_goal_idx].append([c_code[0][0],c_code[0][1] ])
print('Saving models')
self.save_models()
def initialise(self,
goal_size=3,
image_size=64,
channels=1,
batch_size=16,
goal_selection_mode='db',
exp_iteration=0,
hist_size=35,
prob_update=0.1):
self.exp_iteration = exp_iteration
self.goal_size = goal_size
self.image_size = image_size
self.channels = channels
self.samples = []
self.code_size = 32
self.iteration = 0
self.test_data_step = 500
self.test_size = 50
self.max_iterations = 5000
self.history_size = hist_size # how many batches (*batch_size) are kept?
self.history_buffer_update_prob = prob_update # what is the probabilty that a data element in the history buffer is substituted with a new one
self.pos = []
self.cmd = []
self.img = []
self.history_pos = [] # kept to prevent catastrophic forgetting
self.history_img = [] # kept to prevent catastrophic forgetting
self.goal_code = []
self.mse_inv = []
self.mse_fwd = []
self.mse_inv_goal = [] # on goals only
self.mse_fwd_goal = [] # on goals only
self.samples_pos = []
self.samples_img = []
self.samples_codes = []
self.test_positions = []
self.goal_selection_mode = goal_selection_mode # 'som' or 'kmeans'
self.move = False
self.autoencoder, self.encoder, self.decoder = None, None, None
#self.forward_model = None
self.forward_code_model = None
#self.inverse_model = None
self.inverse_code_model = None
self.goal_som = None
self.interest_model = InterestModel(self.goal_size)
self.current_goal_x = -1
self.current_goal_y = -1
self.current_goal_idx = -1
self.cae_epochs = 1
self.inv_epochs = 2
self.fwd_epochs = 2
self.random_cmd_flag = False
self.random_cmd_rate = 0.30
# self.goal_db = ['./sample_images/img_0.jpg','./sample_images/img_200.jpg','./sample_images/img_400.jpg','./sample_images/img_600.jpg','./sample_images/img_800.jpg','./sample_images/img_1000.jpg','./sample_images/img_1200.jpg','./sample_images/img_1400.jpg','./sample_images/img_1600.jpg' ]
self.goal_image = np.zeros(
(1, self.image_size, self.image_size, channels), np.float32)
self.batch_size = batch_size
self.count = 1
self.log_lp = [] # learning progress for each goal
#self.log_lr_inv = [] # learning rate inverse model
self.log_goal_pos = [] # ccurrent goal position (x,y xcarve position)
for i in range(self.goal_size * self.goal_size):
self.log_goal_pos.append([])
self.log_goal_pred = [
] # ccurrent xcarve position (x,y xcarve position)
for i in range(self.goal_size * self.goal_size):
self.log_goal_pred.append([])
self.log_goal_img = [] # ccurrent goal position (x,y xcarve position)
for i in range(self.goal_size * self.goal_size):
self.log_goal_img.append([])
self.log_curr_img = [
] # ccurrent xcarve position (x,y xcarve position)
for i in range(self.goal_size * self.goal_size):
self.log_curr_img.append([])
self.log_goal_id = []
self.log_cvh_inv = []
dataset = '../../rgb_rectified/compressed_dataset.pkl'
print('Loading test dataset ', dataset)
self.train_images, self.test_images, self.train_cmds, self.test_cmds, self.train_pos, self.test_pos = models.load_data(
dataset, self.image_size, step=self.test_data_step)
# load models
self.autoencoder, self.encoder, self.decoder = models.load_autoencoder(
directory='./models/',
code_size=self.code_size,
image_size=self.image_size,
batch_size=self.batch_size,
epochs=self.cae_epochs)
# self.forward_model = models.load_forward_model(train=False)
self.forward_code_model = models.load_forward_code_model(
directory='./models/', code_size=self.code_size, train=False)
#self.inverse_model = models.load_inverse_model(train=False)
self.inverse_code_model = models.load_inverse_code_model(
directory='./models/', code_size=self.code_size, train=False)
if self.goal_selection_mode == 'kmeans':
self.kmeans = models.load_kmeans()
if self.goal_selection_mode == 'som':
self.goal_som = models.load_som(directory='./models/',
encoder=self.encoder,
goal_size=self.goal_size)
# initialise convex hulls (debug stuff)
np.random.seed(
10) # generate always the same random input to the convex hull
pt_inv = []
for i in range(3):
a = np.random.rand(2) * 0.01
pt_inv.append(a)
np.random.seed() # change the seed
self.convex_hull_inv = ConvexHull(np.asarray(pt_inv), incremental=True)
p = Position()
p.x = int(0)
p.y = int(0)
p.z = int(-90)
p.speed = int(1800)
self.prev_pos = p
extract_images_from_pkl(path, compressed_dataset_filename)
else:
print ('Ok. Terminating program')
else:
print ('creating compressed dataset')
samples = []
counter=0
for file in os.listdir(path):
filename_full = os.path.basename(file)
filename = os.path.splitext(filename_full)[0]
splitted = re.split('x|_|y', filename)
p = Position()
p.x=splitted[1]
p.y=splitted[3]
p.z=-90
p.speed=1400
#print path+'/'+os.path.relpath(file)
cv_img = cv2.imread(path+'/'+os.path.relpath(file))
cv_img = cv2.resize(cv_img, (64, 64))
#image_msg= bridge.cv2_to_imgmsg(cv_img, "bgr8")
#samples.append({'image': image_msg, 'position':p, 'command':p})
samples.append({'image': cv_img, 'position': p, 'command': p})
#print int(p.x), ' ', int(p.y)
counter=counter+1
print (counter)
with gzip.open(path+ compressed_dataset_filename, 'wb') as file:
pickle.dump(samples, file, protocol=2)
print ('Compressed dataset saved to ', path+compressed_dataset_filename)
sys.exit(1)