def run_sift(PATH_TO_DATA, count, n_features = 20):
cap = cv2.VideoCapture(PATH_TO_DATA)
sift = cv2.SIFT(nfeatures = n_features)
i = 0
X1 = None
X2 = None
IPython.embed()
while(1):
print str(count) + " "+ str(i)
ret, frame = cap.read()
if not ret:
break;
kp, des = sift.detectAndCompute(frame, None)
img = cv2.drawKeypoints(frame, kp)
cv2.imshow('sift',img)
vector1 = []
vector2 = []
kp.sort(key = lambda x: x.response, reverse = True)
for kp_elem in kp:
vector1 += [kp_elem.response, kp_elem.pt[0], kp_elem.pt[1], kp_elem.size, kp_elem.angle]
vector2 += [kp_elem.pt[0], kp_elem.pt[1]]
# vector2 = utils.reshape(des.flatten())
try:
X1 = utils.safe_concatenate(X1, utils.reshape(np.array(vector1[:n_features * 5])))
X2 = utils.safe_concatenate(X2, utils.reshape(np.array(vector2[:n_features * 2])))
except ValueError as e:
IPython.embed()
cap.release()
cv2.destroyAllWindows()
return X1, X2
def generate_change_points_2(self): """ Generates changespoints by clustering across demonstrations. """ cp_index = 0 for demonstration in self.list_of_demonstrations: W = self.data_W[demonstration] Z = self.data_Z[demonstration] PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p" annotations = pickle.load(open(PATH_TO_ANNOTATION, "rb")) manual_labels = utils.get_chronological_sequences(annotations) start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION) for elem in manual_labels: frm = elem[1] change_pt_W = W[(frm - start)/self.sr] change_pt_Z = Z[(frm - start)/self.sr] change_pt = utils.safe_concatenate(change_pt_W, change_pt_Z) self.append_cp_array(change_pt) self.map_cp2demonstrations[cp_index] = demonstration self.map_cp2frm[cp_index] = frm self.list_of_cp.append(cp_index) cp_index += 1
def parse_kinematics(PATH_TO_KINEMATICS_DATA, PATH_TO_ANNOTATION, fname): """ Takes in PATH to kinematics data (a txt file) and outputs a N x 38 matrix, where N is the number of frames. There are 38 dimensions in the kinematic data 39-41 (3) : Slave left tooltip xyz 42-50 (9) : Slave left tooltip R 51-53 (3) : Slave left tooltip trans_vel x', y', z' 54-56 (3) : Slave left tooltip rot_vel 57 (1) : Slave left gripper angle 58-76 (19): Slave right """ start, end = get_start_end_annotations(PATH_TO_ANNOTATION) X = None if constants.SIMULATION: mat = scipy.io.loadmat(PATH_TO_KINEMATICS_DATA + fname) X = mat['x_traj'] X = X.T # IPython.embed() # X = pickle.load(open(PATH_TO_KINEMATICS_DATA + fname + ".p", "rb")) elif constants.TASK_NAME in ["plane","lego"]: print "-- Parsing Kinematics for ", fname trajectory = pickle.load(open(PATH_TO_KINEMATICS_DATA + fname + ".p", "rb")) for frm in range(start, end + 1): try: traj_point = trajectory[frm - start] except IndexError as e: print e IPython.embed() # vector = list(traj_point.position[16:-12]) + list(traj_point.velocity[16:-12]) X = utils.safe_concatenate(X, utils.reshape(traj_point)) else: X = None all_lines = open(PATH_TO_KINEMATICS_DATA + fname + ".txt", "rb").readlines() i = start - 1 if i < 0: i = 0 while i < end: traj = np.array(all_lines[i].split()) slave = traj[constants.KINEMATICS_DIM:] X = utils.safe_concatenate(X, utils.reshape(slave)) i += 1 return X.astype(np.float)
def generate_transition_features(self): for demonstration in self.list_of_demonstrations: X = self.data_X[demonstration] self.data_X_size[demonstration] = X.shape[1] T = X.shape[0] N = None for t in range(T - self.temporal_window): n_t = utils.make_transition_feature(X, self.temporal_window, t) N = utils.safe_concatenate(N, n_t) self.data_N[demonstration] = N
def construct_features_visual(self): """ Independently loads/sets-up the kinematics in self.data_Z. """ data_X = pickle.load(open(PATH_TO_FEATURES + str(self.featfile),"rb")) for demonstration in self.list_of_demonstrations: X = data_X[demonstration] Z = None for i in range(len(X)): Z = utils.safe_concatenate(Z, utils.reshape(X[i][constants.KINEMATICS_DIM:])) assert Z.shape[0] == X.shape[0] self.data_Z[demonstration] = Z
def generate_transition_features(self):
for demonstration in self.list_of_demonstrations:
X = self.data_X[demonstration]
self.data_X_size[demonstration] = X.shape[1]
T = X.shape[0]
N = None
for t in range(T - self.temporal_window):
n_t = utils.make_transition_feature(X, self.temporal_window, t)
N = utils.safe_concatenate(N, n_t)
self.data_N[demonstration] = N
def construct_features_visual(self):
"""
Independently loads/sets-up the kinematics in self.data_Z.
"""
data_X = pickle.load(open(PATH_TO_FEATURES + str(self.featfile), "rb"))
for demonstration in self.list_of_demonstrations:
X = data_X[demonstration]
Z = None
for i in range(len(X)):
Z = utils.safe_concatenate(
Z, utils.reshape(X[i][constants.KINEMATICS_DIM:]))
assert Z.shape[0] == X.shape[0]
self.data_Z[demonstration] = Z
def generate_sift_features():
list_of_demonstrations = ["plane_9",]
for demonstration in list_of_demonstrations:
print "SIFT for ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
X1 = None
X2 = None
n_features = 20
sift = cv2.SIFT(nfeatures = n_features)
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
# if ((frm % 3) == 0):
PATH_TO_IMAGE = utils.get_full_image_path(constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/", frm)
print PATH_TO_IMAGE
img = cv2.imread(PATH_TO_IMAGE)
kp, des = sift.detectAndCompute(img, None)
img = cv2.drawKeypoints(img, kp)
cv2.imshow('sift',img)
cv2.imwrite('../sift_images/' + demonstration + "/" + str(frm) +".jpg",img)
vector1 = []
vector2 = []
kp.sort(key = lambda x: x.response, reverse = True)
for kp_elem in kp:
vector1 += [kp_elem.response, kp_elem.pt[0], kp_elem.pt[1], kp_elem.size, kp_elem.angle]
vector2 += [kp_elem.pt[0], kp_elem.pt[1]]
try:
X1 = utils.safe_concatenate(X1, utils.reshape(np.array(vector1[:n_features * 5])))
X2 = utils.safe_concatenate(X2, utils.reshape(np.array(vector2[:n_features * 2])))
except ValueError as e:
IPython.embed()
pickle.dump(X1, open("sift_features/SIFT_" + demonstration + "_1.p", "wb"))
pickle.dump(X2, open("sift_features/SIFT_" + demonstration + "_2.p", "wb"))
def loads_features_split(self):
"""
Independently loads/sets-up the kinematics and visual data, then
concatenates to populate self.data_X with X vectors.
"""
self.construct_features_kinematics()
self.construct_features_visual()
for demonstration in self.list_of_demonstrations:
W = self.data_W[demonstration]
Z = self.data_Z[demonstration]
assert W.shape[0] == Z.shape[0]
# assert W.shape[1] == constants.KINEMATICS_DIM
self.data_X[demonstration] = utils.safe_concatenate(W, Z, axis=1)
def loads_features_split(self): """ Independently loads/sets-up the kinematics and visual data, then concatenates to populate self.data_X with X vectors. """ self.construct_features_kinematics() self.construct_features_visual() for demonstration in self.list_of_demonstrations: W = self.data_W[demonstration] Z = self.data_Z[demonstration] assert W.shape[0] == Z.shape[0] # assert W.shape[1] == constants.KINEMATICS_DIM self.data_X[demonstration] = utils.safe_concatenate(W, Z, axis = 1)
def construct_features_visual(self): """ Loads visual features (saved as pickle files) and populates self.data_X dictionary """ data_X = pickle.load(open(PATH_TO_FEATURES + str(self.feat_fname),"rb")) for demonstration in self.list_of_demonstrations: if demonstration not in data_X.keys(): print "[ERROR] Missing demonstrations" sys.exit() X = data_X[demonstration] X_visual = None for i in range(len(X)): X_visual = utils.safe_concatenate(X_visual, utils.reshape(X[i][constants.KINEMATICS_DIM:])) assert X_visual.shape[0] == X.shape[0] self.data_X[demonstration] = X_visual
def construct_features_visual(self): """ Loads visual features (saved as pickle files) and populates self.data_X dictionary """ data_X = pickle.load(open(PATH_TO_FEATURES + str(self.feat_fname),"rb")) for demonstration in self.list_of_demonstrations: if demonstration not in data_X.keys(): print "[ERROR] Missing demonstrations" sys.exit() X = data_X[demonstration] X_visual = None for i in range(len(X)): X_visual = utils.safe_concatenate(X_visual, utils.reshape(X[i][constants.KINEMATICS_DIM:])) assert X_visual.shape[0] == X.shape[0] self.data_X[demonstration] = X_visual
def generate_transition_features(self): """ For each data point X(t), transition feature are created as follows: N(t) = X(t) + X(t+1) + .. + X(T), where T is self.temporal_window. """ self.X_dimension = self.data_X[self.list_of_demonstrations[0]].shape[1] print "X dimension", str(self.X_dimension) for demonstration in self.list_of_demonstrations: X = self.data_X[demonstration] T = X.shape[0] N = None for t in range(T - self.temporal_window): n_t = utils.make_transition_feature(X, self.temporal_window, t) N = utils.safe_concatenate(N, n_t) self.data_N[demonstration] = N
def generate_transition_features(self): """ For each data point X(t), transition feature are created as follows: N(t) = X(t) + X(t+1) + .. + X(T), where T is self.temporal_window. """ self.X_dimension = self.data_X[self.list_of_demonstrations[0]].shape[1] print "X dimension", str(self.X_dimension) for demonstration in self.list_of_demonstrations: X = self.data_X[demonstration] T = X.shape[0] N = None for t in range(T - self.temporal_window): n_t = utils.make_transition_feature(X, self.temporal_window, t) N = utils.safe_concatenate(N, n_t) self.data_N[demonstration] = N
def cluster_pruning(self):
for cluster in self.map_level1_cp.keys():
cluster_list_of_cp = self.map_level1_cp[cluster]
cluster_demonstrations = []
for cp in cluster_list_of_cp:
cluster_demonstrations.append(self.map_cp2demonstrations[cp])
data_representation = float(len(set(cluster_demonstrations))) / float(len(self.list_of_demonstrations))
weighted_data_representation = pruning.weighted_score(
self.list_of_demonstrations, list(set(cluster_demonstrations))
)
print str(cluster) + ": " + str(data_representation), " " + str(len(cluster_list_of_cp))
print str(cluster) + ":w " + str(weighted_data_representation), " " + str(len(cluster_list_of_cp))
val = weighted_data_representation if constants.WEIGHTED_PRUNING_MODE else data_representation
if val <= self.representativeness:
print "Pruned"
new_cluster_list = cluster_list_of_cp[:]
print "Pruned cluster"
for cp in cluster_list_of_cp:
self.list_of_cp.remove(cp)
new_cluster_list.remove(cp)
self.map_level1_cp[cluster] = new_cluster_list
predictions = []
filtered_changepoints = None
inv_map = {v: k for k, v in constants.alphabet_map.items()}
for cluster in self.map_level1_cp:
cluster_list_of_cp = self.map_level1_cp[cluster]
for cp in cluster_list_of_cp:
predictions.append(inv_map[cluster])
filtered_changepoints = utils.safe_concatenate(
filtered_changepoints, utils.reshape(self.changepoints[cp])
)
predictions = np.array(predictions)
self.save_cluster_metrics(filtered_changepoints, predictions, "level1")
def cluster_pruning(self):
for cluster in self.map_level1_cp.keys():
cluster_list_of_cp = self.map_level1_cp[cluster]
cluster_demonstrations = []
for cp in cluster_list_of_cp:
cluster_demonstrations.append(self.map_cp2demonstrations[cp])
data_representation = float(len(set(cluster_demonstrations))) / float(len(self.list_of_demonstrations))
weighted_data_representation = pruning.weighted_score(self.list_of_demonstrations, list(set(cluster_demonstrations)))
print str(cluster) + ": " + str(data_representation), " " + str(len(cluster_list_of_cp))
print str(cluster) + ":w " + str(weighted_data_representation), " " + str(len(cluster_list_of_cp))
val = weighted_data_representation if constants.WEIGHTED_PRUNING_MODE else data_representation
if val <= self.representativeness:
print "Pruned"
new_cluster_list = cluster_list_of_cp[:]
print "Pruned cluster"
for cp in cluster_list_of_cp:
self.list_of_cp.remove(cp)
new_cluster_list.remove(cp)
self.map_level1_cp[cluster] = new_cluster_list
predictions = []
filtered_changepoints = None
inv_map = {v:k for k, v in constants.alphabet_map.items()}
for cluster in self.map_level1_cp:
cluster_list_of_cp = self.map_level1_cp[cluster]
for cp in cluster_list_of_cp:
predictions.append(inv_map[cluster])
filtered_changepoints = utils.safe_concatenate(filtered_changepoints, utils.reshape(self.changepoints[cp]))
predictions = np.array(predictions)
self.save_cluster_metrics(filtered_changepoints, predictions, 'level1')
def generate_raw_image_pixels(list_of_demonstrations):
"""
PCA and t-SNE on raw image pixels
"""
# Design matrix of raw image pixels
X = None
for demonstration in list_of_demonstrations:
print "Raw image pixels ", demonstration
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
for frm in range(start, end + 1):
if ((frm % 6) == 0):
PATH_TO_IMAGE = utils.get_full_image_path(constants.PATH_TO_DATA + constants.NEW_FRAMES_FOLDER + demonstration + "_" + constants.CAMERA + "/", frm)
print demonstration, str(frm)
img = utils.reshape(cv2.imread(PATH_TO_IMAGE).flatten())
X = utils.safe_concatenate(X, img)
X_pca = utils.pca(X, PC = 2)
X_tsne = utils.tsne(X)
data_dimred = [X_pca, X_tsne]
pickle.dump(X_tsne, open("raw_pixel_" + demonstration + "_dimred.p", "wb"))
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = parser.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER
+ demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generating Changepoints. Fitting GMM/DP-GMM ..."
if constants.REMOTE == 1:
if self.fit_DPGMM:
print "Init DPGMM"
avg_len = int(big_N.shape[0]/len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(avg_len/constants.DPGMM_DIVISOR)
print "L0", DP_GMM_COMPONENTS, "ALPHA: ", self.ALPHA_CP
dpgmm = mixture.DPGMM(n_components = DP_GMM_COMPONENTS, covariance_type='diag', n_iter = 10000, alpha = self.ALPHA_CP, thresh= 1e-7)
if self.fit_GMM:
print "Init GMM"
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', n_iter=5000, thresh = 5e-5)
if constants.REMOTE == 2:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', thresh = 0.01)
else:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
if self.fit_GMM:
print "Fitting GMM"
start = time.time()
gmm.fit(big_N)
end = time.time()
print "GMM Time:", end - start
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
print "Fitting DPGMM"
start = time.time()
dpgmm.fit(big_N)
end = time.time()
print "DPGMM Time:", end - start
Y_dpgmm = dpgmm.predict(big_N)
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
Y = Y_dpgmm
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][self.X_dimension:]
self.append_cp_array(utils.reshape(change_pt))
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points, " + str(cp_index)
"plane_3_js.p", "plane_4_js.p", "plane_5_js.p", "plane_6_js.p",
"plane_7_js.p", "plane_8_js.p", "plane_9_js.p", "plane_10_js.p"
]
list_of_trajectories = [
"plane_3.p", "plane_4.p", "plane_5.p", "plane_6.p", "plane_7.p",
"plane_8.p", "plane_9.p", "plane_10.p"
]
list_of_annotations = [
"plane_3_capture2.p", "plane_4_capture2.p", "plane_5_capture2.p",
"plane_6_capture2.p", "plane_7_capture2.p", "plane_8_capture2.p",
"plane_9_capture2.p", "plane_10_capture2.p"
]
for i in range(len(list_of_annotations)):
print list_of_annotations[i], list_of_joint_states[
i], list_of_trajectories[i]
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA +
"annotations/" +
list_of_annotations[i])
X = None
trajectory = pickle.load(
open(constants.PATH_TO_KINEMATICS + list_of_joint_states[i], "rb"))
for frm in range(start, end + 1):
traj_point = trajectory[frm]
print traj_point.velocity[16:-12]
vector = list(traj_point.position[16:-12]) + list(
traj_point.velocity[16:-12])
X = utils.safe_concatenate(X, utils.reshape(np.array(vector)))
# pickle.dump(X, open(constants.PATH_TO_KINEMATICS + list_of_trajectories[i],"wb"))
def start_recording(self):
print "Recorder Loop"
while self.left_image is None or self.right_image is None:
pass
if self.record_kinematics:
while (1):
try:
(trans,rot) = self.listener.lookupTransform('/r_gripper_tool_frame', '/base_link', rospy.Time(0))
break
except (tf.ExtrapolationException):
print "ExtrapolationException"
rospy.sleep(0.1)
continue
frm = 0
wait_thresh = 0
prev_r_l = self.r_l
prev_r_r = self.r_r
trans_vel = np.array([0.0, 0.0, 0.0])
rot_vel = np.array([0.0, 0.0, 0.0])
prev_trans = None
prev_rot = None
for i in range(9999999):
print frm
rospy.sleep(self.period)
start = time.time()
cv2.imwrite(self.video_folder + self.task_name + "_" + self.trial_name + "_capture1/" + str(get_frame_fig_name(frm)), self.left_image)
cv2.imwrite(self.video_folder + self.task_name + "_" + self.trial_name + "_capture2/" + str(get_frame_fig_name(frm)), self.right_image)
if self.record_kinematics:
(trans, quaternion) = self.listener.lookupTransform('/r_gripper_tool_frame', '/base_link', rospy.Time(0))
r_matrix = utils.quaternion2rotation(quaternion)
rot = transformations.euler_from_matrix(r_matrix)
r_gripper_angle = self.joint_state.position[-17]
if frm != 0:
trans_vel = (trans - prev_trans) / self.period
rot_vel = (rot - prev_rot) / self.period
prev_trans = np.array(trans)
prev_rot = np.array(rot)
js_pos = self.joint_state.position[16:-12]
js_vel = self.joint_state.velocity[16:-12]
W = list(trans) + list(r_matrix.flatten()) + list(trans_vel) + list(rot_vel)
# Gripper angle is r_gripper_joint
W.append(r_gripper_angle)
W = W + list(js_pos) + list(js_vel)
self.data = utils.safe_concatenate(self.data, utils.reshape(np.array(W)))
frm += 1
if ((self.r_l == prev_r_l) and (self.r_r == prev_r_r)):
print "Not recording anymore?"
wait_thresh += 1
if wait_thresh > 5:
self.save_and_quit()
prev_r_l = self.r_l
prev_r_r = self.r_r
end = time.time()
print end - start
def featurize_LCD_VLAD(list_of_demonstrations,
kinematics,
layer,
net_name,
folder,
dimensions,
batch_size,
fname,
config=[True, True, True]):
M = dimensions[0]
a = dimensions[1]
print "Featurizing LCD + VLAD: ", layer, net_name, folder, M, a, batch_size
BATCH_SIZE = batch_size
if constants.SIMULATION:
BATCH_SIZE = 5
data_X_PCA = {}
data_X_CCA = {}
data_X_GRP = {}
size_sampled_matrices = [
utils.sample_matrix(kinematics[demo],
sampling_rate=BATCH_SIZE).shape[0]
for demo in list_of_demonstrations
]
PC = min(100, min(size_sampled_matrices))
print "PC: ", PC
for demonstration in list_of_demonstrations:
print demonstration
W = kinematics[demonstration]
Z = load_cnn_features(demonstration, layer, folder, net_name)
W_new = utils.sample_matrix(W, sampling_rate=BATCH_SIZE)
Z_batch = None
W_batch = None
j = 1
Z_new = None
IPython.embed()
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(
constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
Z = []
IPython.embed()
for i in range(Z):
vector_W = W[i]
W_batch = utils.safe_concatenate(W_batch, vector_W)
vector_Z = Z[i]
vector_Z = vector_Z.reshape(M, a, a)
vector_Z = lcd.LCD(vector_Z)
Z_batch = utils.safe_concatenate(Z_batch, vector_Z)
if (j == BATCH_SIZE):
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
# Re-initialize batch variables
j = 0
Z_batch = None
W_batch = None
j += 1
# tail case
if Z_batch is not None:
print "TAIL CASE"
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
if config[0]:
Z_new_pca = utils.pca_incremental(Z_new, PC=PC)
print Z_new_pca.shape
assert W_new.shape[0] == Z_new_pca.shape[0]
X_PCA = np.concatenate((W_new, Z_new_pca), axis=1)
data_X_PCA[demonstration] = X_PCA
if config[1]:
Z_new_cca = utils.cca(W_new, Z_new)
print Z_new_cca.shape
assert W_new.shape[0] == Z_new_cca.shape[0]
X_CCA = np.concatenate((W_new, Z_new_cca), axis=1)
data_X_CCA[demonstration] = X_CCA
if config[2]:
Z_new_grp = utils.grp(Z_new)
print Z_new_grp.shape
assert W_new.shape[0] == Z_new_grp.shape[0]
X_GRP = np.concatenate((W_new, Z_new_grp), axis=1)
data_X_GRP[demonstration] = X_GRP
if config[0]:
pickle.dump(data_X_PCA,
open(PATH_TO_FEATURES + fname + "_PCA" + ".p", "wb"))
if config[1]:
pickle.dump(data_X_CCA,
open(PATH_TO_FEATURES + fname + "_CCA" + ".p", "wb"))
if config[2]:
pickle.dump(data_X_GRP,
open(PATH_TO_FEATURES + fname + "_GRP" + ".p", "wb"))
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
size_of_X = self.data_X_size[self.list_of_demonstrations[0]]
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(
constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER +
demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generated big_N"
if constants.REMOTE == 1:
if self.fit_GMM:
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full',
thresh=0.01)
if self.fit_DPGMM:
# dpgmm = mixture.DPGMM(n_components = 100, covariance_type='diag', n_iter = 10000, alpha = 100, thresh= 2e-4)
#DO NOT FIDDLE WITH PARAMS WITHOUT CONSENT :)
avg_len = int(big_N.shape[0] /
len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(
avg_len / constants.DPGMM_DIVISOR
) #tuned with suturing experts only for kinematics
print "L0 ", DP_GMM_COMPONENTS, "ALPHA: ", constants.ALPHA_ZW_CP
dpgmm = mixture.DPGMM(n_components=DP_GMM_COMPONENTS,
covariance_type='diag',
n_iter=1000,
alpha=constants.ALPHA_ZW_CP,
thresh=1e-7)
elif constants.REMOTE == 2:
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full')
else:
gmm = mixture.GMM(n_components=self.n_components_cp,
covariance_type='full')
if self.fit_GMM:
start = time.time()
gmm.fit(big_N)
end = time.time()
"GMM time taken: ", str(end - start)
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
start = time.time()
dpgmm.fit(big_N)
end = time.time()
"DP-GMM time taken: ", str(end - start)
Y_dpgmm = dpgmm.predict(big_N)
Y = Y_dpgmm
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][:size_of_X]
self.append_cp_array(change_pt)
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[
w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points", len(self.list_of_cp)
def start_recording(self):
print "Recorder Loop"
while self.left_image is None or self.right_image is None:
pass
if self.record_kinematics:
while 1:
try:
(trans, rot) = self.listener.lookupTransform("/r_gripper_tool_frame", "/base_link", rospy.Time(0))
break
except (tf.ExtrapolationException):
print "ExtrapolationException"
rospy.sleep(0.1)
continue
frm = 0
wait_thresh = 0
prev_r_l = self.r_l
prev_r_r = self.r_r
trans_vel = np.array([0.0, 0.0, 0.0])
rot_vel = np.array([0.0, 0.0, 0.0])
prev_trans = None
prev_rot = None
for i in range(9999999):
print frm
rospy.sleep(self.period)
start = time.time()
cv2.imwrite(
self.video_folder
+ self.task_name
+ "_"
+ self.trial_name
+ "_capture1/"
+ str(get_frame_fig_name(frm)),
self.left_image,
)
cv2.imwrite(
self.video_folder
+ self.task_name
+ "_"
+ self.trial_name
+ "_capture2/"
+ str(get_frame_fig_name(frm)),
self.right_image,
)
if self.record_kinematics:
(trans, quaternion) = self.listener.lookupTransform(
"/r_gripper_tool_frame", "/base_link", rospy.Time(0)
)
r_matrix = utils.quaternion2rotation(quaternion)
rot = transformations.euler_from_matrix(r_matrix)
r_gripper_angle = self.joint_state.position[-17]
if frm != 0:
trans_vel = (trans - prev_trans) / self.period
rot_vel = (rot - prev_rot) / self.period
prev_trans = np.array(trans)
prev_rot = np.array(rot)
js_pos = self.joint_state.position[16:-12]
js_vel = self.joint_state.velocity[16:-12]
W = list(trans) + list(r_matrix.flatten()) + list(trans_vel) + list(rot_vel)
# Gripper angle is r_gripper_joint
W.append(r_gripper_angle)
W = W + list(js_pos) + list(js_vel)
self.data = utils.safe_concatenate(self.data, utils.reshape(np.array(W)))
frm += 1
if (self.r_l == prev_r_l) and (self.r_r == prev_r_r):
print "Not recording anymore?"
wait_thresh += 1
if wait_thresh > 5:
self.save_and_quit()
prev_r_l = self.r_l
prev_r_r = self.r_r
end = time.time()
print end - start
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
size_of_X = self.data_X_size[self.list_of_demonstrations[0]]
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER
+ demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generated big_N"
if constants.REMOTE == 1:
if self.fit_GMM:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', thresh = 0.01)
if self.fit_DPGMM:
# dpgmm = mixture.DPGMM(n_components = 100, covariance_type='diag', n_iter = 10000, alpha = 100, thresh= 2e-4)
#DO NOT FIDDLE WITH PARAMS WITHOUT CONSENT :)
avg_len = int(big_N.shape[0]/len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(avg_len/constants.DPGMM_DIVISOR) #tuned with suturing experts only for kinematics
print "L0 ", DP_GMM_COMPONENTS, "ALPHA: ", constants.ALPHA_ZW_CP
dpgmm = mixture.DPGMM(n_components = DP_GMM_COMPONENTS, covariance_type='diag', n_iter = 1000, alpha = constants.ALPHA_ZW_CP, thresh= 1e-7)
elif constants.REMOTE == 2:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
else:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
if self.fit_GMM:
start = time.time()
gmm.fit(big_N)
end = time.time()
"GMM time taken: ", str(end - start)
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
start = time.time()
dpgmm.fit(big_N)
end = time.time()
"DP-GMM time taken: ", str(end - start)
Y_dpgmm = dpgmm.predict(big_N)
Y = Y_dpgmm
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][:size_of_X]
self.append_cp_array(change_pt)
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points", len(self.list_of_cp)
import numpy as np import pickle import constants import utils import parser list_of_joint_states = ["plane_3_js.p", "plane_4_js.p", "plane_5_js.p", "plane_6_js.p", "plane_7_js.p", "plane_8_js.p", "plane_9_js.p", "plane_10_js.p"] list_of_trajectories = ["plane_3.p", "plane_4.p", "plane_5.p", "plane_6.p", "plane_7.p", "plane_8.p", "plane_9.p", "plane_10.p"] list_of_annotations = ["plane_3_capture2.p", "plane_4_capture2.p", "plane_5_capture2.p", "plane_6_capture2.p", "plane_7_capture2.p", "plane_8_capture2.p", "plane_9_capture2.p", "plane_10_capture2.p"] for i in range(len(list_of_annotations)): print list_of_annotations[i], list_of_joint_states[i], list_of_trajectories[i] start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + "annotations/" + list_of_annotations[i]) X = None trajectory = pickle.load(open(constants.PATH_TO_KINEMATICS + list_of_joint_states[i], "rb")) for frm in range(start, end + 1): traj_point = trajectory[frm] print traj_point.velocity[16:-12] vector = list(traj_point.position[16:-12]) + list(traj_point.velocity[16:-12]) X = utils.safe_concatenate(X, utils.reshape(np.array(vector))) # pickle.dump(X, open(constants.PATH_TO_KINEMATICS + list_of_trajectories[i],"wb"))
def append_cp_array(self, cp): self.changepoints = utils.safe_concatenate(self.changepoints, cp)
def append_cp_array(self, cp): self.changepoints = utils.safe_concatenate(self.changepoints, cp)
def generate_change_points_2(self):
"""
Generates changespoints by clustering across demonstrations.
"""
cp_index = 0
i = 0
big_N = None
map_index2demonstration = {}
map_index2frm = {}
for demonstration in self.list_of_demonstrations:
print demonstration
N = self.data_N[demonstration]
start, end = utils.get_start_end_annotations(constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER
+ demonstration + "_" + constants.CAMERA + ".p")
for j in range(N.shape[0]):
map_index2demonstration[i] = demonstration
map_index2frm[i] = start + j * self.sr
i += 1
big_N = utils.safe_concatenate(big_N, N)
print "Generating Changepoints. Fitting GMM/DP-GMM ..."
if constants.REMOTE == 1:
if self.fit_DPGMM:
print "Init DPGMM"
avg_len = int(big_N.shape[0]/len(self.list_of_demonstrations))
DP_GMM_COMPONENTS = int(avg_len/constants.DPGMM_DIVISOR)
print "L0", DP_GMM_COMPONENTS, "ALPHA: ", self.ALPHA_CP
dpgmm = mixture.DPGMM(n_components = DP_GMM_COMPONENTS, covariance_type='diag', n_iter = 10000, alpha = self.ALPHA_CP, thresh= 1e-7)
if self.fit_GMM:
print "Init GMM"
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', n_iter=5000, thresh = 5e-5)
if constants.REMOTE == 2:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full', thresh = 0.01)
else:
gmm = mixture.GMM(n_components = self.n_components_cp, covariance_type='full')
if self.fit_GMM:
print "Fitting GMM"
start = time.time()
gmm.fit(big_N)
end = time.time()
print "GMM Time:", end - start
Y_gmm = gmm.predict(big_N)
print "L0: Clusters in GMM", len(set(Y_gmm))
Y = Y_gmm
if self.fit_DPGMM:
print "Fitting DPGMM"
start = time.time()
dpgmm.fit(big_N)
end = time.time()
print "DPGMM Time:", end - start
Y_dpgmm = dpgmm.predict(big_N)
print "L0: Clusters in DP-GMM", len(set(Y_dpgmm))
Y = Y_dpgmm
for w in range(len(Y) - 1):
if Y[w] != Y[w + 1]:
change_pt = big_N[w][:self.X_dimension]
self.append_cp_array(utils.reshape(change_pt))
self.map_cp2frm[cp_index] = map_index2frm[w]
self.map_cp2demonstrations[cp_index] = map_index2demonstration[w]
self.list_of_cp.append(cp_index)
cp_index += 1
print "Done with generating change points, " + str(cp_index)
def featurize_LCD_VLAD(list_of_demonstrations, kinematics, layer, net_name, folder, dimensions, batch_size, fname, config = [True, True, True]):
M = dimensions[0]
a = dimensions[1]
print "Featurizing LCD + VLAD: ", layer, net_name, folder, M, a, batch_size
BATCH_SIZE = batch_size
if constants.SIMULATION:
BATCH_SIZE = 5
data_X_PCA = {}
data_X_CCA = {}
data_X_GRP = {}
size_sampled_matrices = [utils.sample_matrix(kinematics[demo], sampling_rate = BATCH_SIZE).shape[0] for demo in list_of_demonstrations]
PC = min(100, min(size_sampled_matrices))
print "PC: ", PC
for demonstration in list_of_demonstrations:
print demonstration
W = kinematics[demonstration]
Z = load_cnn_features(demonstration, layer, folder, net_name)
W_new = utils.sample_matrix(W, sampling_rate = BATCH_SIZE)
Z_batch = None
W_batch = None
j = 1
Z_new = None
IPython.embed()
PATH_TO_ANNOTATION = constants.PATH_TO_DATA + constants.ANNOTATIONS_FOLDER + demonstration + "_" + str(constants.CAMERA) + ".p"
start, end = utils.get_start_end_annotations(PATH_TO_ANNOTATION)
Z = []
IPython.embed()
for i in range(Z):
vector_W = W[i]
W_batch = utils.safe_concatenate(W_batch, vector_W)
vector_Z = Z[i]
vector_Z = vector_Z.reshape(M, a, a)
vector_Z = lcd.LCD(vector_Z)
Z_batch = utils.safe_concatenate(Z_batch, vector_Z)
if (j == BATCH_SIZE):
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
# Re-initialize batch variables
j = 0
Z_batch = None
W_batch = None
j += 1
# tail case
if Z_batch is not None:
print "TAIL CASE"
print "NEW BATCH", str(i)
Z_batch_VLAD = encoding.encode_VLAD(Z_batch)
Z_new = utils.safe_concatenate(Z_new, Z_batch_VLAD)
if config[0]:
Z_new_pca = utils.pca_incremental(Z_new, PC = PC)
print Z_new_pca.shape
assert W_new.shape[0] == Z_new_pca.shape[0]
X_PCA = np.concatenate((W_new, Z_new_pca), axis = 1)
data_X_PCA[demonstration] = X_PCA
if config[1]:
Z_new_cca = utils.cca(W_new, Z_new)
print Z_new_cca.shape
assert W_new.shape[0] == Z_new_cca.shape[0]
X_CCA = np.concatenate((W_new, Z_new_cca), axis = 1)
data_X_CCA[demonstration] = X_CCA
if config[2]:
Z_new_grp = utils.grp(Z_new)
print Z_new_grp.shape
assert W_new.shape[0] == Z_new_grp.shape[0]
X_GRP = np.concatenate((W_new, Z_new_grp), axis = 1)
data_X_GRP[demonstration] = X_GRP
if config[0]:
pickle.dump(data_X_PCA, open(PATH_TO_FEATURES + fname + "_PCA" + ".p", "wb"))
if config[1]:
pickle.dump(data_X_CCA, open(PATH_TO_FEATURES + fname + "_CCA" + ".p", "wb"))
if config[2]:
pickle.dump(data_X_GRP, open(PATH_TO_FEATURES + fname + "_GRP" + ".p", "wb"))
