def read_delta_dataset(filename):
delta_vector = []
lines = open(filename, 'r').readlines()
for line in lines:
values_vector = line[:-2].split(',') ## remove final , and EOL
current_delta = delta.Delta(values_vector[0], float(values_vector[1]),
float(values_vector[2]),
float(values_vector[3]),
float(values_vector[4]),
float(values_vector[5]),
float(values_vector[6]), [
float(values_vector[7]),
float(values_vector[8]),
float(values_vector[9]),
float(values_vector[10]),
float(values_vector[11]),
float(values_vector[12])
])
delta_vector.append(current_delta)
# print("in discretize_trajs :")
# current_delta.print_me()
return delta_vector
def create_discr_trajs(nb_initial_pos):
''' Big circle '''
# generate circle positions
list_x_axis, list_y_axis, list_z_axis = \
setup.gen_init_eef(nb_initial_pos)
if sim_param.print_directed_dataset_extended:
''' Plot canvas '''
fig = plt.gcf()
fig.set_size_inches(5, 5)
ax = fig.add_axes([0, 0, 1, 1])
# set axis limits
plt.xlim(-1.2, 1.2)
plt.ylim(-1.2, 1.2)
# plot the origin
origin = Rectangle((sim_param.obj_pos[0] - sim_param.obj_side / 2,
sim_param.obj_pos[1] - sim_param.obj_side / 2),
sim_param.obj_side,
sim_param.obj_side,
fc="grey")
ax.add_patch(origin)
# ax.axis('off')
# plot the big circle points
ax.plot(list_x_axis, list_y_axis, 'o', c='r')
''' Generate the trajs '''
## Generate predefined mid pos around the box
obj_pos = sim_param.obj_pos
mid_pos_dist = sim_param.obj_side + sim_param.obj_side / 2
pos_t_l = [obj_pos[0] - mid_pos_dist, obj_pos[1] + mid_pos_dist]
pos_t_c = [obj_pos[0], obj_pos[1] + mid_pos_dist]
pos_t_r = [obj_pos[0] + mid_pos_dist, obj_pos[1] + mid_pos_dist]
pos_m_l = [obj_pos[0] - mid_pos_dist, obj_pos[1]]
pos_m_r = [obj_pos[0] + mid_pos_dist, obj_pos[1]]
pos_d_l = [obj_pos[0] - mid_pos_dist, obj_pos[1] - mid_pos_dist]
pos_d_c = [obj_pos[0], obj_pos[1] - mid_pos_dist]
pos_d_r = [obj_pos[0] + mid_pos_dist, obj_pos[1] - mid_pos_dist]
## MID_POINT = [POS, [related TRAJS] ## Generate predefined trajs to the center of the obj
## First, the directed trajs, from the sides
# trajs_steps = 3
''' top-center '''
trajs_steps = setup.compute_nb_wps(pos_t_c, obj_pos[-1])
traj_t_c_y = list(np.linspace(pos_t_c[1], obj_pos[1], trajs_steps))
traj_t_c_x = [obj_pos[0] for i in range(len(traj_t_c_y))]
traj_t_c = Traj(traj_t_c_x, traj_t_c_y, 'down')
t_c = Predef_pos(pos_t_c[0], pos_t_c[1], [traj_t_c])
''' mid_left '''
traj_m_l_x = list(np.linspace(pos_m_l[0], obj_pos[0], trajs_steps))
traj_m_l_y = [obj_pos[1] for i in range(len(traj_m_l_x))]
traj_m_l = Traj(traj_m_l_x, traj_m_l_y, 'right')
m_l = Predef_pos(pos_m_l[0], pos_m_l[1], [traj_m_l])
''' mid_right '''
traj_m_r_x = list(np.linspace(pos_m_r[0], obj_pos[0], trajs_steps))
traj_m_r_y = [obj_pos[1] for i in range(len(traj_m_r_x))]
traj_m_r = Traj(traj_m_r_x, traj_m_r_y, 'left')
m_r = Predef_pos(pos_m_r[0], pos_m_r[1], [traj_m_r])
''' down-center '''
traj_d_c_y = list(np.linspace(pos_d_c[1], obj_pos[1], trajs_steps))
traj_d_c_x = [obj_pos[0] for i in range(len(traj_d_c_y))]
traj_d_c = Traj(traj_d_c_x, traj_d_c_y, 'up')
d_c = Predef_pos(pos_d_c[0], pos_d_c[1], [traj_d_c])
## Now the extended trajs from the corners to the sides,
## and from there to the center of the box
''' top-left '''
## top-left + top-center
traj_t_l_t_c_x = list(np.linspace(pos_t_l[0], pos_t_c[0],
trajs_steps)) + traj_t_c_x
# traj_t_l_t_c_y = [pos_t_l[1] for i in range(trajs_steps)] + traj_t_c_y
traj_t_l_t_c_y = [
pos_t_l[1] for i in range(len(traj_t_l_t_c_x) - len(traj_t_c_y))
] + traj_t_c_y
traj_t_l_t_c = Traj(traj_t_l_t_c_x, traj_t_l_t_c_y, 'down')
# ax.plot(traj_t_l_t_c_x, traj_t_l_t_c_y, '-')
## top-left + mid-left
# traj_t_l_m_l_x = [pos_t_l[0] for i in range(trajs_steps)] + traj_m_l_x
traj_t_l_m_l_y = list(np.linspace(pos_t_l[1], pos_m_l[1],
trajs_steps)) + traj_m_l_y
traj_t_l_m_l_x = [
pos_t_l[0] for i in range(len(traj_t_l_m_l_y) - len(traj_m_l_x))
] + traj_m_l_x
traj_t_l_m_l = Traj(traj_t_l_m_l_x, traj_t_l_m_l_y, 'right')
# ax.plot(traj_t_l_m_l_x, traj_t_l_m_l_y, '-')
t_l = Predef_pos(pos_t_l[0], pos_t_l[1], [traj_t_l_t_c, traj_t_l_m_l])
''' top-right '''
## top-right + top-center
traj_t_r_t_c_x = list(np.linspace(pos_t_r[0], pos_t_c[0],
trajs_steps)) + traj_t_c_x
# traj_t_r_t_c_y = [pos_t_r[1] for i in range(trajs_steps)] + traj_t_c_y
traj_t_r_t_c_y = [
pos_t_r[1] for i in range(len(traj_t_r_t_c_x) - len(traj_t_c_y))
] + traj_t_c_y
traj_t_r_t_c = Traj(traj_t_r_t_c_x, traj_t_r_t_c_y, 'down')
# ax.plot(traj_t_r_t_c_x, traj_t_r_t_c_y, '-')
## top-right + mid-right
# traj_t_r_m_r_x = [pos_t_r[0] for i in range(trajs_steps)] + traj_m_r_x
traj_t_r_m_r_y = list(np.linspace(pos_t_r[1], pos_m_r[1],
trajs_steps)) + traj_m_r_y
traj_t_r_m_r_x = [
pos_t_r[0] for i in range(len(traj_t_r_m_r_y) - len(traj_m_r_x))
] + traj_m_r_x
traj_t_r_m_r = Traj(traj_t_r_m_r_x, traj_t_r_m_r_y, 'left')
# ax.plot(traj_t_r_m_r_x, traj_t_r_m_r_y, '-')
t_r = Predef_pos(pos_t_r[0], pos_t_r[1], [traj_t_r_t_c, traj_t_r_m_r])
''' down-left '''
## down-left + down-center
traj_d_l_d_c_x = list(np.linspace(pos_d_l[0], pos_d_c[0],
trajs_steps)) + traj_d_c_x
# traj_d_l_d_c_y = [pos_d_l[1] for i in range(trajs_steps)] + traj_d_c_y
traj_d_l_d_c_y = [
pos_d_l[1] for i in range(len(traj_d_l_d_c_x) - len(traj_d_c_y))
] + traj_d_c_y
traj_d_l_d_c = Traj(traj_d_l_d_c_x, traj_d_l_d_c_y, 'up')
# ax.plot(traj_d_l_d_c_x, traj_d_l_d_c_y, '-')
## down-left + mid-left
# traj_d_l_m_l_x = [pos_d_l[0] for i in range(trajs_steps)] + traj_m_l_x
traj_d_l_m_l_y = list(np.linspace(pos_d_l[1], pos_m_l[1],
trajs_steps)) + traj_m_l_y
traj_d_l_m_l_x = [
pos_d_l[0] for i in range(len(traj_d_l_m_l_y) - len(traj_m_l_x))
] + traj_m_l_x
traj_d_l_m_l = Traj(traj_d_l_m_l_x, traj_d_l_m_l_y, 'right')
# ax.plot(traj_d_l_m_l_x, traj_d_l_m_l_y, '-')
d_l = Predef_pos(pos_d_l[0], pos_d_l[1], [traj_d_l_d_c, traj_d_l_m_l])
''' down-right '''
## down-right + down-center
traj_d_r_d_c_x = list(np.linspace(pos_d_r[0], pos_d_c[0],
trajs_steps)) + traj_d_c_x
# traj_d_r_d_c_y = [pos_d_r[1] for i in range(trajs_steps)] + traj_d_c_y
traj_d_r_d_c_y = [
pos_d_r[1] for i in range(len(traj_d_r_d_c_x) - len(traj_d_c_y))
] + traj_d_c_y
traj_d_r_d_c = Traj(traj_d_r_d_c_x, traj_d_r_d_c_y, 'up')
# ax.plot(traj_d_r_d_c_x, traj_d_r_d_c_y, '-')
## down-right + mid-right
# traj_d_r_m_r_x = [pos_d_r[0] for i in range(trajs_steps)] + traj_m_r_x
traj_d_r_m_r_y = list(np.linspace(pos_d_r[1], pos_m_r[1],
trajs_steps)) + traj_m_r_y
traj_d_r_m_r_x = [
pos_d_r[0] for i in range(len(traj_d_r_m_r_y) - len(traj_m_r_x))
] + traj_m_r_x
traj_d_r_m_r = Traj(traj_d_r_m_r_x, traj_d_r_m_r_y, 'left')
# ax.plot(traj_d_r_m_r_x, traj_d_r_m_r_y, '-')
d_r = Predef_pos(pos_d_r[0], pos_d_r[1], [traj_d_r_d_c, traj_d_r_m_r])
## Set of available predefined pos and trajs
predefined_pos_traj_vector = [t_l, t_c, t_r, m_l, m_r, d_l, d_c, d_r]
## From each init pos, create trajs to the predefined points
## If these trajs do not contact the box, extend them with the
## predefined trajs from each predefined point
delta_vector = []
save_fig_id = 0
for init_pos in range(len(list_x_axis)):
# for init_pos in range(0,1):
## for each predefined pos around the obj
# if init_pos == 0:
# t_l = Predef_pos(pos_t_l[0], pos_t_l[1], [traj_t_l_t_c])
# t_r = Predef_pos(pos_t_r[0], pos_t_r[1], [])
# d_l = Predef_pos(pos_d_l[0], pos_d_l[1], [traj_d_l_d_c, traj_d_l_m_l])
# d_r = Predef_pos(pos_d_r[0], pos_d_r[1], [traj_d_r_m_r])
#
#
# elif init_pos == 1:
# t_l = Predef_pos(pos_t_l[0], pos_t_l[1], [traj_t_l_t_c])
# t_r = Predef_pos(pos_t_r[0], pos_t_r[1], [traj_t_r_t_c])
# d_l = Predef_pos(pos_d_l[0], pos_d_l[1], [traj_d_l_m_l])
# d_r = Predef_pos(pos_d_r[0], pos_d_r[1], [traj_d_r_m_r])
#
#
# elif init_pos == 2:
# t_l = Predef_pos(pos_t_l[0], pos_t_l[1], [])
# t_r = Predef_pos(pos_t_r[0], pos_t_r[1], [traj_t_r_t_c])
# d_l = Predef_pos(pos_d_l[0], pos_d_l[1], [traj_d_l_m_l])
# d_r = Predef_pos(pos_d_r[0], pos_d_r[1], [traj_d_r_d_c, traj_d_r_m_r])
#
# predefined_pos_traj_vector = [t_l, t_c, t_r, m_l, m_r, d_l, d_c, d_r]
for current_pred_pos_traj in predefined_pos_traj_vector:
## create a traj to it
trajs_steps = setup.compute_nb_wps(
[list_x_axis[init_pos], list_y_axis[init_pos]],
[current_pred_pos_traj.get_x(),
current_pred_pos_traj.get_y()])
traj_x = list(
np.linspace(list_x_axis[init_pos],
current_pred_pos_traj.get_x(), trajs_steps))
traj_y = list(
np.linspace(list_y_axis[init_pos],
current_pred_pos_traj.get_y(), trajs_steps))
# ax.plot(traj_x, traj_y, '-')
# ax.plot(traj_x, traj_y, 'b*')
## Check contact of each wp with object
current_init_traj = list(zip(traj_x, traj_y))
obj_moved = False
current_wp_pos = 0
while not obj_moved and current_wp_pos < len(current_init_traj) - 1:
current_x = current_init_traj[current_wp_pos][0]
current_y = current_init_traj[current_wp_pos][1]
next_x = current_init_traj[current_wp_pos + 1][0]
next_y = current_init_traj[current_wp_pos + 1][1]
## compute new box pos
updated_obj_pos = env.compute_obj_pos(
[current_x, current_y, 0], [next_x, next_y, 0])
obj_moved = True \
if updated_obj_pos != sim_param.obj_pos else False
current_wp_pos += 1
## If no interaction with the obj, extend the traj
## with the predefined ones
if not obj_moved:
current_extended_traj_vector = []
pred_traj_vector = current_pred_pos_traj.get_traj_vector()
for pred_traj in pred_traj_vector:
current_extended_traj = Traj(
traj_x + pred_traj.get_traj_x(),
traj_y + pred_traj.get_traj_y(),
pred_traj.get_effect())
current_extended_traj_vector.append(current_extended_traj)
if sim_param.print_directed_dataset_extended: # and init_pos == 7:
ax.plot(current_extended_traj.get_traj_x(),
current_extended_traj.get_traj_y(), '-*')
# plt.savefig('save_fig_' + str(save_fig_id) + '.png')
# save_fig_id += 1
## For each created extended trajectory, split it up into deltas
current_delta_vector = []
for ext_traj in current_extended_traj_vector:
ext_traj_x = ext_traj.get_traj_x()
ext_traj_y = ext_traj.get_traj_y()
for ext_wp_pos in range(len(ext_traj_x) - 1):
current_x = ext_traj_x[ext_wp_pos]
current_y = ext_traj_y[ext_wp_pos]
next_x = ext_traj_x[ext_wp_pos + 1]
next_y = ext_traj_y[ext_wp_pos + 1]
updated_obj_pos = env.compute_obj_pos(
[current_x, current_y, 0], [next_x, next_y, 0])
current_delta = delta.Delta(
ext_traj.get_effect(), current_x, current_y, 0,
next_x, next_y, 0, sim_param.obj_pos[0],
sim_param.obj_pos[1], 0, updated_obj_pos[0],
updated_obj_pos[1], 0,
True) ## Extended trajs are always moving the obj
current_delta_vector.append(current_delta)
delta_vector += current_delta_vector
if sim_param.print_directed_dataset_extended:
plt.show()
return delta_vector
def create_discr_trajs(nb_initial_pos, single_init_pos=False, single_pos=0):
''' Get all object pos'''
obj_pos_dict = OrderedDict()
for obj_name in sim_param.obj_name_vector:
obj_pos = ros_services.call_get_model_state(obj_name)
obj_pos = [round(pos, sim_param.round_value) for pos in obj_pos[0:3]]
obj_pos_dict[obj_name] = obj_pos
''' Big circle '''
# generate circle positions
list_x_axis, list_y_axis, list_z_axis = \
setup.gen_init_eef(nb_initial_pos,
sim_param.untucked_left_eef_pos[0] - obj_pos_dict[sim_param.obj_name_vector[0]][0],
obj_pos_dict[sim_param.obj_name_vector[0]])
if __name__ == '__main__':
''' Plot canvas '''
fig = plt.figure()
fig.set_size_inches(6, 6)
ax = fig.add_axes([0, 0, 1, 1])
# set axis limits
lim = 0.1 + sim_param.untucked_left_eef_pos[0] - obj_pos_dict[
sim_param.obj_name_vector[0]][0]
plt.xlim(obj_pos_dict["cube"][0] - lim, obj_pos_dict["cube"][0] + lim)
plt.ylim(obj_pos_dict["cube"][1] - lim, obj_pos_dict["cube"][1] + lim)
# ax.axis('off')
# plot the origin
origin = Rectangle((obj_pos_dict["cube"][0] - sim_param.cube_x / 2,
obj_pos_dict["cube"][1] - sim_param.cube_y / 2),
sim_param.cube_x,
sim_param.cube_y,
fc="grey")
ax.add_patch(origin)
# plot the big circle points
ax.plot(list_x_axis, list_y_axis, 'o', c='r')
''' Generate the trajs '''
it = 0
step_length = sim_param.step_length
step_options = [-step_length, 0, step_length]
nb_max_box_touched_found = False
nb_box_touched = 0
delta_vector = []
while not nb_max_box_touched_found and it < 50000:
if not single_init_pos:
loop_range = (0, len(list_x_axis))
## if single_init_pos only generate trajs in pos single_pos
else:
loop_range = (single_pos, single_pos + 1)
## for each initial position one traj is created each time
# for p in range(len(list_x_axis)):
# for p in range(5,6):
# for p in range(0,1):
for p in range(loop_range[0], loop_range[1]):
## compute each step of the trajectory, called delta
x_i = [list_x_axis[p]]
y_i = [list_y_axis[p]]
nb_delta = 0
obj_moved = False
current_delta_vector = []
effect = 'None'
while nb_delta < sim_param.random_max_movs and \
not obj_moved: # only N movs (delta) allowed
current_x = x_i[-1]
current_y = y_i[-1]
if sim_param.semi_random_trajs:
new_x = current_x + random.choice(step_options)
new_y = current_y + random.choice(step_options)
else:
new_x = current_x + random.uniform(-step_length,
step_length)
new_y = current_y + random.uniform(-step_length,
step_length)
x_i.append(new_x)
y_i.append(new_y)
## compute new box pos
updated_obj_pos = env.compute_obj_pos(
obj_pos_dict["cube"],
[current_x, current_y, obj_pos_dict["cube"][2]],
[new_x, new_y, obj_pos_dict["cube"][2]])
updated_obj_pos = updated_obj_pos[1]
obj_moved = True \
if updated_obj_pos != obj_pos_dict["cube"] else False
## store current delta if there was a move
if current_x != new_x or \
current_y != new_y:
current_delta = delta.Delta(
effect,
current_x,
current_y,
0,
new_x,
new_y,
0,
[
obj_pos_dict["cube"][0],
obj_pos_dict["cube"][1],
obj_pos_dict["cube"][2], ## constant
updated_obj_pos[0],
updated_obj_pos[1],
obj_pos_dict["cube"][2]
]) ## constant
# ,
# obj_moved)
current_delta_vector.append(current_delta)
# print(len(current_delta_vector))
if obj_moved:
## stop generating wps if max trajs inferred
nb_box_touched += 1
if nb_box_touched == \
nb_initial_pos * len(sim_param.effect_values):
nb_max_box_touched_found = True
## compute related effect
effect = discr.compute_effect(obj_pos_dict["cube"],
updated_obj_pos)
# print(updated_obj_pos, effect)
for d in current_delta_vector:
d.set_effect(effect)
nb_delta += 1
## print traj
if __name__ == "__main__" and obj_moved:
ax.plot(x_i, y_i, '-')
ax.plot(x_i, y_i, '*', c='grey')
## only store trajs contacting the box
if obj_moved:
delta_vector = delta_vector + current_delta_vector
# print('len delta_vector', len(delta_vector))
it += 1
if __name__ == "__main__":
plt.show()
return delta_vector
def create_effect_trajs(nb_initial_pos, single_pos, desired_effect,
dataset_size):
''' Big circle '''
# generate circle positions
list_x_axis, list_y_axis, list_z_axis = \
setup.gen_init_eef(nb_initial_pos)
# if sim_param.print_discr_random_dataset:
if __name__ == '__main__':
''' Plot canvas '''
fig = plt.figure()
fig.set_size_inches(7, 7)
ax = fig.add_axes([0, 0, 1, 1])
# set axis limits
plt.xlim(-1.2, 1.2)
plt.ylim(-1.2, 1.2)
# ax.axis('off')
# plot the origin
origin = Rectangle((sim_param.obj_pos[0] - sim_param.obj_side / 2,
sim_param.obj_pos[1] - sim_param.obj_side / 2),
sim_param.obj_side,
sim_param.obj_side,
fc="grey")
ax.add_patch(origin)
# plot the big circle points
ax.plot(list_x_axis, list_y_axis, 'o', c='r')
''' Generate the trajs '''
it = 0
step_length = sim_param.step_length
step_options = [-step_length, 0, step_length]
delta_vector = []
desired_effect_achieved = False
current_nb_desired_effect = 0
while not desired_effect_achieved: # and \
# it < 50000:
## compute each step of the trajectory, called delta
x_i = [list_x_axis[single_pos]]
y_i = [list_y_axis[single_pos]]
nb_delta = 0
obj_moved = False
current_delta_vector = []
effect = 'None'
## new trajectory
while nb_delta < sim_param.random_max_movs and \
not obj_moved: # only N movs (delta) allowed
current_x = x_i[-1]
current_y = y_i[-1]
new_x = current_x + random.uniform(-step_length * 2,
step_length * 2)
new_y = current_y + random.uniform(-step_length * 2,
step_length * 2)
# new_x = current_x + random.choice(step_options)
# new_y = current_y + random.choice(step_options)
x_i.append(new_x)
y_i.append(new_y)
## compute new box pos
updated_obj_pos = env.compute_obj_pos([current_x, current_y, 0],
[new_x, new_y, 0])
obj_moved = True \
if updated_obj_pos != sim_param.obj_pos else False
## store current delta if there was a move
if current_x != new_x or \
current_y != new_y:
current_delta = delta.Delta(effect, current_x, current_y, 0,
new_x, new_y, 0,
sim_param.obj_pos[0],
sim_param.obj_pos[1], 0,
updated_obj_pos[0],
updated_obj_pos[1], 0, obj_moved)
current_delta_vector.append(current_delta)
# print(len(current_delta_vector))
if obj_moved:
## compute related effect
effect = discr.compute_effect(updated_obj_pos)
for d in current_delta_vector:
d.set_effect(effect)
nb_delta += 1
## print traj
if __name__ == "__main__" and obj_moved and effect == desired_effect:
ax.plot(x_i, y_i, '-')
ax.plot(x_i, y_i, '*', c='grey')
## only store trajs contacting the box
if obj_moved and effect == desired_effect:
current_nb_desired_effect += 1
delta_vector = delta_vector + current_delta_vector
if current_nb_desired_effect == \
sim_param.nb_desired_effect_reproduced:
# dataset_size * 0.01:
print('dataset_size', dataset_size)
desired_effect_achieved = True
it += 1
if __name__ == "__main__":
plt.show()
print(current_nb_desired_effect)
return delta_vector
def create_discr_trajs(nb_initial_pos,
# left_gripper_interface,
write_dataset_bool = True,
single_init_pos = False,
single_pos = 0):
''' Restart scenario '''
success = ros_services.call_restart_world("all")
if not success:
print("ERROR - restart_world failed")
''' Generate the trajs '''
it = 0
step_length = sim_param.step_length
# step_options = [-step_length, 0, step_length]
nb_max_box_touched_found = False
nb_box_touched = 0
delta_vector = []
## Nb of initial positions from where generate a traj
if not single_init_pos:
init_pos_range = range(0,sim_param.nb_min_init_pos)
else: ## if single_init_pos only generate trajs in pos single_pos
init_pos_range = range(single_pos, single_pos + 1)
thrs = sim_param.obj_moved_threshold
traj_vector = []
while not nb_max_box_touched_found and it < 5000*1:
## for each initial position one traj is created each time
# for p in range(5,6):
# for p in range(0,1):
for curr_init_pos in init_pos_range:
''' Get object pos'''
obj_pos_dict = OrderedDict()
obj_pos = ros_services.call_get_model_state("cube")
obj_pos = [round(pos, sim_param.round_value) for pos in obj_pos[0:3]]
obj_pos_dict["cube"] = obj_pos
''' Big circle '''
list_x_axis, list_y_axis, list_z_axis = \
setup.gen_init_eef(nb_initial_pos,
sim_param.radio,
obj_pos_dict[sim_param.obj_name_vector[0]])
if __name__ == '__main__':
ax = plot_setup([obj_pos_dict["cube"]],
[list_x_axis,
list_y_axis,
list_z_axis])
ax.view_init(90,0) # top view
## compute each step of the trajectory, called delta
x_i = [list_x_axis[curr_init_pos]]
y_i = [list_y_axis[curr_init_pos]]
z_i = [list_z_axis[curr_init_pos]]
wp_vector = [list_x_axis[curr_init_pos],
list_y_axis[curr_init_pos],
list_z_axis[curr_init_pos],
1,1,1] ## fake eef orientation
wp_vector += obj_pos_dict["cube"]
wp_vector += [2,2,2] ## fake obj orientation
sim_obj_moved = False
real_obj_moved = False
current_delta_vector = []
real_effect = ''
nb_delta = 0
while nb_delta < sim_param.random_max_movs and \
not real_obj_moved: # only N movs (delta) allowed
print(curr_init_pos, nb_delta)
current_x = x_i[-1]
current_y = y_i[-1]
current_z = z_i[-1]
# if sim_param.semi_random_trajs:
# new_x = round(current_x + random.choice(step_options), sim_param.round_value)
# new_y = round(current_y + random.choice(step_options), sim_param.round_value)
## new_z = current_z + random.choice(step_options)
# else:
# new_x = round(current_x + random.uniform(-step_length,step_length), sim_param.round_value)
# new_y = round(current_y + random.uniform(-step_length,step_length), sim_param.round_value)
## new_z = current_z + random.uniform(-step_length,step_length)
if sim_param.semi_random_trajs:
var_x = random.choice([-step_length, 0, step_length])
# if var_x != 0:
# var_y = 0
# else:
var_y = random.choice([-step_length, 0, step_length])
# new_z = round(current_z + random.choice(step_options), sim_param.round_value)
else:
var_x = random.uniform(-step_length/2,step_length/2)
var_y = random.uniform(-step_length/2,step_length/2)
# new_z = round(current_z + random.uniform(-step_length,step_length), sim_param.round_value)
# new_x = round(current_x + var_x, sim_param.round_value)
# new_y = round(current_y + var_y, sim_param.round_value)
## normalize mov size
var_vector = [var_x, var_y]
# dims_vector = 0
# for value in var_vector:
# if value != 0:
# dims_vector += 1
# if dims_vector == 0:
# dims_vector = 1
dims_vector = 1
var_vector = [round(value/sqrt(dims_vector), sim_param.round_value)
for value in var_vector]
# print(var_vector)
new_x = round(current_x + var_vector[0], sim_param.round_value)
new_y = round(current_y + var_vector[1], sim_param.round_value)
############################################################################## TODO OJO !!!
new_z = current_z
x_i.append(new_x)
y_i.append(new_y)
z_i.append(new_z)
## compute new box pos
sim_final_obj_pos = env.compute_obj_pos(
obj_pos_dict["cube"],
[current_x, current_y, current_z],
[new_x, new_y, new_z])
sim_final_obj_pos = sim_final_obj_pos[1]
wp_vector += [new_x, new_y, new_z, 1,1,1]
wp_vector += sim_final_obj_pos
wp_vector += [2,2,2] ## fake obj orientation
sim_obj_moved = True \
if (abs(obj_pos_dict["cube"][0] - sim_final_obj_pos[0]) +
abs(obj_pos_dict["cube"][1] - sim_final_obj_pos[1]) +
abs(obj_pos_dict["cube"][2] - sim_final_obj_pos[2])) >= thrs else False
# if sim_final_obj_pos != obj_pos_dict["cube"] else False
## store current delta if the eef moved
if current_x != new_x or \
current_y != new_y:
current_delta = delta.Delta(
real_effect,
current_x,current_y,current_z,
new_x, new_y, new_z,
[obj_pos_dict["cube"][0],
obj_pos_dict["cube"][1],
obj_pos_dict["cube"][2],
sim_final_obj_pos[0],
sim_final_obj_pos[1],
sim_final_obj_pos[2]])
current_delta_vector.append(current_delta)
## check if movement in real robot
if sim_obj_moved:
## replicate last move
x_i.append(round(new_x + var_x, sim_param.round_value))
y_i.append(round(new_y + var_y, sim_param.round_value))
z_i.append(new_z)
# replicated_delta = delta.Delta(
# '',
# new_x, new_y, new_z,
# new_x + var_x, new_y + var_y, new_z,
# [obj_pos_dict["cube"][0], obj_pos_dict["cube"][1], obj_pos_dict["cube"][2],
# sim_final_obj_pos[0], sim_final_obj_pos[1], sim_final_obj_pos[2]])
# current_delta_vector.append(replicated_delta)
## eef to init pos
''' Update eef pos'''
init_pos_coord = [x_i[0], y_i[0], z_i[0]]
success = ros_services.call_move_to_initial_position(init_pos_coord)
if not success:
print("ERROR - call_move_to_initial_position failed")
eef_pos = ros_services.call_get_eef_pose('left')
eef_pos = [round(pos, sim_param.round_value) for pos in eef_pos[0:3]]
# left_gripper_interface.close()
initial_obj_pos = obj_pos_dict["cube"]
## execute real mov to get real effect
simulated_traj = zip(x_i, y_i, z_i)
simulated_traj_vector = [i for el in simulated_traj for i in el] ## [float]
ros_services.call_trajectory_motion(sim_param.feedback_window,
simulated_traj_vector)
''' Get object pos'''
obj_pos_dict = OrderedDict()
obj_pos = ros_services.call_get_model_state("cube")
obj_pos = [round(pos, sim_param.round_value) for pos in obj_pos[0:3]]
obj_pos_dict["cube"] = obj_pos
real_final_obj_pos = obj_pos
real_obj_moved = \
(abs(initial_obj_pos[0] - real_final_obj_pos[0]) +
abs(initial_obj_pos[1] - real_final_obj_pos[1]) +
abs(initial_obj_pos[2] - real_final_obj_pos[2])) >= thrs * 3
## store current delta if there was a real contact with the box
if real_obj_moved:
real_effect = discr.compute_effect(initial_obj_pos,
real_final_obj_pos)
print("real_effect", real_effect)
if real_effect != '' and real_effect != None:
## stop generating wps if max trajs inferred
nb_box_touched += 1
print('nb_box_touched', nb_box_touched)
if single_init_pos:
tmp_nb_box_touched = 1
else:
tmp_nb_box_touched = nb_initial_pos
if nb_box_touched == \
tmp_nb_box_touched * len(sim_param.effect_values) * 16:
nb_max_box_touched_found = True
## print traj
if __name__ == "__main__" and sim_obj_moved:
# ax = plot_setup([initial_obj_pos],
# [list_x_axis,
# list_y_axis,
# list_z_axis])
ax.plot(x_i, y_i, z_i,
'-',
linewidth = 8)
ax.plot(x_i, y_i, z_i, '*', c='grey')
## set real effect
for d in current_delta_vector:
d.set_effect(real_effect)
## set real final obj pos
# last_delta = current_delta_vector[-2]
# last_delta.update_obj_final(real_final_obj_pos)
# current_delta_vector[-2] = last_delta
last_delta = current_delta_vector[-1]
last_delta.update_obj_final(real_final_obj_pos)
current_delta_vector[-1] = last_delta
wp_vector[-6] = real_final_obj_pos[0]
wp_vector[-5] = real_final_obj_pos[1]
wp_vector[-4] = real_final_obj_pos[2]
delta_vector = delta_vector + current_delta_vector
traj_vector.append(wp_vector)
# for ddd in current_delta_vector:
# ddd.print_me()
## update cube pos
if distance.euclidean(real_final_obj_pos,
sim_param.first_obj_pos) > \
sim_param.obj_too_far_distance:
print('-------------> UPDATING CUBE POSITION!')
## move arm up
eef_pos = ros_services.call_get_eef_pose('left')
eef_pos = [round(pos, sim_param.round_value) for pos in eef_pos[0:3]]
pos_up = copy.copy(eef_pos)
pos_up[2] += 0.2
success = ros_services.call_move_to_initial_position(pos_up)
if not success:
print("ERROR - extend dataset failed")
## compute new obj pos
new_obj_pos = sim_param.first_obj_pos
new_obj_pos = [new_obj_pos[0] + random.uniform(-sim_param.new_obj_pos_dist,
sim_param.new_obj_pos_dist),
new_obj_pos[1] + random.uniform(-sim_param.new_obj_pos_dist,
sim_param.new_obj_pos_dist),
new_obj_pos[2]]
new_obj_pos = [round(poss, sim_param.round_value) for poss in new_obj_pos]
## move obj to new pos
success = ros_services.call_restart_world("object",
sim_param.obj_name_vector[0],
new_obj_pos)
if not success:
print("ERROR - restart_world failed for object", sim_param.obj_name_vector[0])
nb_delta += 1
plt.close()
it += 1
if __name__ == "__main__":
plt.show()
if write_dataset_bool:
# for ddd in delta_vector:
# ddd.print_me()
write_dataset(traj_vector,
delta_vector)
return delta_vector
def read_dataset(filename):
# mean_move_size = 0
nb_moves = 0
delta_vector = []
lines = open(filename, 'r').readlines()
for line in lines:
current_delta_vector = []
pos_rot_vector = line[:-2].split(',') ## remove final , and EOL
nb_obj = len(sim_param.obj_name_vector) ## expected
# nb_dataset_obj = sim_param.dataset_nb_objects ## actual
obj_initial_pos = [
float(pos_rot_vector[6]),
float(pos_rot_vector[7]),
float(pos_rot_vector[8])
]
obj_final_pos = [
float(pos_rot_vector[-6 - 6 * (nb_obj - 1)]),
float(pos_rot_vector[-5 - 6 * (nb_obj - 1)]),
float(pos_rot_vector[-4 - 6 * (nb_obj - 1)])
]
obtained_effect = discr.compute_effect(obj_initial_pos, obj_final_pos)
related_info_size = 6 + 6 * nb_obj
for pos in range(0,
len(pos_rot_vector) - related_info_size,
related_info_size):
current_x = float(pos_rot_vector[pos + 0])
current_y = float(pos_rot_vector[pos + 1])
current_z = float(pos_rot_vector[pos + 2])
next_x = float(pos_rot_vector[pos + related_info_size + 0])
next_y = float(pos_rot_vector[pos + related_info_size + 1])
next_z = float(pos_rot_vector[pos + related_info_size + 2])
# mean_move_size += d.euclidean([current_x, current_y, current_z],
# [next_x, next_y, next_z])
nb_moves += 1
current_next_obj_pos_vector = []
for curr_obj_id in range(1, len(sim_param.obj_name_vector) + 1):
current_obj_pos_x = float(pos_rot_vector[pos +
6 * curr_obj_id + 0])
current_obj_pos_y = float(pos_rot_vector[pos +
6 * curr_obj_id + 1])
current_obj_pos_z = float(pos_rot_vector[pos +
6 * curr_obj_id + 2])
next_obj_pos_x = float(pos_rot_vector[pos + related_info_size +
6 * curr_obj_id + 0])
next_obj_pos_y = float(pos_rot_vector[pos + related_info_size +
6 * curr_obj_id + 1])
next_obj_pos_z = float(pos_rot_vector[pos + related_info_size +
6 * curr_obj_id + 2])
# current_next_obj_pos = [current_obj_pos_x, current_obj_pos_y,current_obj_pos_z,
# next_obj_pos_x, next_obj_pos_y, next_obj_pos_z]
# current_next_obj_pos_vector.append(current_next_obj_pos)
current_next_obj_pos_vector += [
current_obj_pos_x, current_obj_pos_y, current_obj_pos_z,
next_obj_pos_x, next_obj_pos_y, next_obj_pos_z
]
current_delta = delta.Delta(obtained_effect, current_x, current_y,
current_z, next_x, next_y, next_z,
current_next_obj_pos_vector)
current_delta_vector.append(current_delta)
delta_vector += current_delta_vector
# mean_move_size /= nb_moves
# sim_param.step_length = round(mean_move_size, sim_param.round_value)
print('Move step length:', sim_param.step_length)
return delta_vector
def extend_trajectory(
current_traj_vector, ## [WPs]
current_delta_vector, ## [delta_class]
nb_initial_pos, ## 8
current_init_pos, ## 0,1,2,3,..., 7
expected_effect,
# new_random_directed_trajs, ## bool
dataset_size,
current_trajs_dict,
actions_to_learn_vector,
left_gripper_interface):
print('\n\n------------------->', current_init_pos, expected_effect)
## move arm up
eef_pos = ros_services.call_get_eef_pose('left')
if eef_pos[2] < 0:
eef_pos = [round(pos, sim_param.round_value) for pos in eef_pos[0:3]]
pos_up = copy.copy(eef_pos)
pos_up[2] += 0.2
success = ros_services.call_move_to_initial_position(pos_up)
if not success:
print("ERROR - extend dataset failed")
initial_obj_pos = current_delta_vector[0].get_obj_init()
init_eef_pos = [
current_delta_vector[0].get_wp_init().get_x(),
current_delta_vector[0].get_wp_init().get_y(),
current_delta_vector[0].get_wp_init().get_z()
]
## move obj to new pos
success = ros_services.call_restart_world("object",
sim_param.obj_name_vector[0],
initial_obj_pos)
step_length = sim_param.step_length
new_delta_trajs_vector = []
nb_trajs_found = 0
''' generate max N new trajs '''
thrs = sim_param.obj_moved_threshold
# nb_new_traj = 0
counter = 0
nb_inferred_tries = 0
while nb_inferred_tries < sim_param.max_nb_inferred_tries and \
nb_trajs_found < sim_param.extend_max_trajs and \
counter < 10000:
## for (almost) each WP of the traj
curr_wp = 1
''' For each WP '''
while curr_wp < len(current_traj_vector):
## new tmp traj from the beginning to this WP
tmp_traj = current_traj_vector[:-curr_wp]
tmp_delta = current_delta_vector[:-curr_wp]
traj_vector_x = [v[0] for v in tmp_traj]
traj_vector_y = [v[1] for v in tmp_traj]
traj_vector_z = [v[2] for v in tmp_traj]
nb_new_delta = 0
sim_obj_moved = False
real_obtained_effect = ''
extended_delta_vector = tmp_delta
''' Generate new traj from a WP'''
## extend with new random moves from the closest to the more far one
## the more far is the WP respect the object, the more deltas can generate
while nb_new_delta < (sim_param.extend_max_movs + (curr_wp-1)) and \
not sim_obj_moved:
current_x = traj_vector_x[-1]
current_y = traj_vector_y[-1]
current_z = traj_vector_z[-1]
if sim_param.semi_random_trajs:
var_x = random.choice([-step_length, 0, 0, step_length])
# if var_x != 0:
# var_y = 0
# else:
var_y = random.choice([-step_length, 0, 0, step_length])
# new_z = round(current_z + random.choice(step_options), sim_param.round_value)
else:
var_x = random.uniform(-step_length / 2, step_length / 2)
var_y = random.uniform(-step_length / 2, step_length / 2)
# new_z = round(current_z + random.uniform(-step_length,step_length), sim_param.round_value)
# new_x = round(current_x + var_x, sim_param.round_value)
# new_y = round(current_y + var_y, sim_param.round_value)
## normalize mov size
var_vector = [var_x, var_y]
print(var_vector)
# dims_vector = 0
# for value in var_vector:
# if value != 0:
# dims_vector += 1
# if dims_vector == 0:
# dims_vector = 1
dims_vector = 1
var_vector = [
round(value / sqrt(dims_vector), sim_param.round_value)
for value in var_vector
]
new_x = round(current_x + var_vector[0], sim_param.round_value)
new_y = round(current_y + var_vector[1], sim_param.round_value)
############################################################################## TODO OJO !!!
new_z = current_z
traj_vector_x.append(new_x)
traj_vector_y.append(new_y)
traj_vector_z.append(new_z)
# print("\nDelta :", var_vector[0], var_vector[1],
# 'Norm:', round(d.euclidean([0, 0],
# [var_vector[0], var_vector[1]]), 3))
# print('Previous EEF pos :',
# current_x, current_y, current_z)
# print('New EEF pos :',
# new_x, new_y, new_z)
## check if obj moved and compute new box pos
sim_initial_obj_pos = initial_obj_pos #obj_pos_dict['cube']
sim_obj_moved, sim_final_obj_pos = \
env.compute_obj_pos(sim_initial_obj_pos,
[current_x,current_y,current_z],
[new_x, new_y, new_z])
## store current delta
current_delta = delta.Delta(
'', ## effect
current_x,
current_y,
current_z,
new_x,
new_y,
new_z,
[
sim_initial_obj_pos[0], sim_initial_obj_pos[1],
sim_initial_obj_pos[2], sim_final_obj_pos[0],
sim_final_obj_pos[1], sim_final_obj_pos[2]
])
extended_delta_vector.append(current_delta)
nb_new_delta += 1
## end generate new deltas
curr_wp += 1
if sim_obj_moved:
# if var_x == 0 or var_y == 0 :
## replicate last move
traj_vector_x.append(
round(new_x + var_x, sim_param.round_value))
traj_vector_y.append(
round(new_y + var_y, sim_param.round_value))
traj_vector_z.append(new_z)
replicated_delta = delta.Delta(
'', new_x, new_y, new_z, new_x + var_x, new_y + var_y,
new_z, [
sim_initial_obj_pos[0], sim_initial_obj_pos[1],
sim_initial_obj_pos[2], sim_final_obj_pos[0],
sim_final_obj_pos[1], sim_final_obj_pos[2]
])
extended_delta_vector.append(replicated_delta)
nb_new_delta += 1
## compute related effect
sim_obtained_effect = discr.compute_effect(
sim_initial_obj_pos, sim_final_obj_pos)
# if sim_obtained_effect in actions_to_learn_vector:
if sim_obtained_effect != '' and sim_obtained_effect != None:
# if True:
print('\nsim_obtained_effect', sim_obtained_effect)
print('--> real - extending for', current_init_pos,
sim_obtained_effect)
nb_inferred_tries += 1
print('nb_inferred_tries', nb_inferred_tries)
## print simulated extended traj
print(current_init_pos, expected_effect, "- WP", curr_wp,
"- extension", nb_new_delta)
plot_sim_extended_traj(initial_obj_pos, nb_initial_pos,
traj_vector_x, traj_vector_y,
traj_vector_z)
''' Update eef pos'''
ros_services.call_move_to_initial_position(
[0.65, 0.1, 0.1])
init_pos_coord = init_eef_pos
success = ros_services.call_move_to_initial_position(
init_pos_coord)
if not success:
print("ERROR - call_move_to_initial_position failed")
eef_pos = ros_services.call_get_eef_pose('left')
eef_pos = [
round(pos, sim_param.round_value)
for pos in eef_pos[0:3]
]
left_gripper_interface.close()
real_initial_obj_pos = initial_obj_pos #obj_pos_dict["cube"]
## execute real mov to get real effect
simulated_traj_vector = zip(traj_vector_x, traj_vector_y,
traj_vector_z)
simulated_traj_vector = [
i for el in simulated_traj_vector for i in el
] ## [float]
res_exec = ros_services.call_trajectory_motion(
sim_param.feedback_window, simulated_traj_vector)
''' Get object pos'''
obj_pos_dict = OrderedDict()
obj_pos = ros_services.call_get_model_state("cube")
obj_pos = [
round(pos, sim_param.round_value)
for pos in obj_pos[0:3]
]
obj_pos_dict["cube"] = obj_pos
real_final_obj_pos = obj_pos
real_obj_moved = \
(abs(real_initial_obj_pos[0] - real_final_obj_pos[0]) +
abs(real_initial_obj_pos[1] - real_final_obj_pos[1]) +
abs(real_initial_obj_pos[2] - real_final_obj_pos[2])) >= thrs
print('real_obj_moved', real_obj_moved)
print('delta X',
real_final_obj_pos[0] - real_initial_obj_pos[0])
print('delta Y',
real_final_obj_pos[1] - real_initial_obj_pos[1])
## store current delta if there was a real contact with the box
if real_obj_moved:
# nb_inferred_tries += 1
# print('nb_inferred_tries', nb_inferred_tries)
## compute real effect
real_obtained_effect = discr.compute_effect(
real_initial_obj_pos, real_final_obj_pos)
print('real_obtained_effect', real_obtained_effect)
if real_obtained_effect != '': # and \
# real_obtained_effect in actions_to_learn_vector:
nb_trajs_found += 1
print('nb_trajs_found', nb_trajs_found)
## only store the traj if (init_pos, obtained_effect)
## is not successful
# obtained_traj_class = \
# current_trajs_dict[current_init_pos, real_obtained_effect]
# obtained_traj_res = obtained_traj_class.get_traj_res()
# if obtained_traj_res != 'success':
# print('-------------> STORE NEW (', current_init_pos, real_obtained_effect, ') FOUND!')
if True:
for dd in extended_delta_vector:
dd.set_effect(real_obtained_effect)
# dd.print_me()
new_delta_trajs_vector += extended_delta_vector
## stop looking for trajs for this init_pos effect
# actions_to_learn_vector = [effect for effect in actions_to_learn_vector
# if effect != real_obtained_effect]
## move arm up
eef_pos = ros_services.call_get_eef_pose('left')
eef_pos = [
round(pos, sim_param.round_value)
for pos in eef_pos[0:3]
]
if eef_pos[2] < -0.1: ## not previously up
print('move eef up')
pos_up = copy.copy(eef_pos)
pos_up[2] += 0.2
success = ros_services.call_move_to_initial_position(
pos_up)
if not success:
print(
"ERROR - extend dataset failed - move eef up")
## compute new obj pos
# new_obj_pos = sim_param.first_obj_pos
# new_obj_pos = [new_obj_pos[0] + random.uniform(-sim_param.new_obj_pos_dist,
# sim_param.new_obj_pos_dist),
# new_obj_pos[1] + random.uniform(-sim_param.new_obj_pos_dist,
# sim_param.new_obj_pos_dist),
# new_obj_pos[2]]
# new_obj_pos = [round(poss, sim_param.round_value) for poss in new_obj_pos]
## move obj to init pos
success = ros_services.call_restart_world(
"object", sim_param.obj_name_vector[0],
initial_obj_pos)
if not success:
print("ERROR - restart_world failed for object",
sim_param.obj_name_vector[0])
# else:
# obj_pos_dict['cube'] = new_obj_pos
# nb_new_traj += 1
counter += 1
## end generate traj
print(nb_trajs_found, "new trajs found")
return new_delta_trajs_vector, actions_to_learn_vector