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 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
def simulate_traj(bn, ie, eef_pos, obj_pos_dict, expected_effect,
current_orien, current_inclin, current_dist):
eef_traj = [[
round(eef_pos[0], sim_param.round_value),
round(eef_pos[1], sim_param.round_value),
round(eef_pos[2], sim_param.round_value)
]]
all_obj_moved = False
mean_traj_prob = 0
i = 0
while not all_obj_moved and i < sim_param.inferred_max_moves:
if sim_param.debug_infer:
print('\nInferred delta ', i)
current_eef_x = eef_traj[-1][0] ## last value added
current_eef_y = eef_traj[-1][1]
current_eef_z = eef_traj[-1][2]
virt_inference_vector = []
if sim_param.experiment_type != 'a2l_reproduce_dataset':
## compute current variables value
node_names = ['effect']
node_values = [expected_effect]
obj_id = 0
for obj_name in sim_param.obj_name_vector:
# print('obj pos inferring -->', obj_name, obj_pos_dict[obj_name])
distance = discr_dist.compute_distance(
[current_eef_x, current_eef_y], obj_pos_dict[obj_name],
current_dist)
orientation = discr_orien.compute_orientation_discr(
[current_eef_x, current_eef_y], obj_pos_dict[obj_name],
current_orien)
if sim_param.inclination_param:
inclination = discr_inclin.compute_inclination_discr(
obj_pos_dict[obj_name],
[current_eef_x, current_eef_y, current_eef_z],
current_inclin)
node_names += [
'distance' + str(obj_id), 'orientation' + str(obj_id),
'inclination' + str(obj_id)
]
node_values += [distance, orientation, inclination]
else:
node_names += [
'distance' + str(obj_id), 'orientation' + str(obj_id)
]
node_values += [distance, orientation]
obj_id += 1
## infere next move
try:
next_mov_discr, prob_value, delta_nb_var, same_prob = \
infer.infere_mov(bn, ie,
node_names,
node_values)
mean_traj_prob += prob_value
# if sim_param.debug_infer:
print(node_values, next_mov_discr, prob_value)
print('Next move :', next_mov_discr.upper(), 'for',
node_values, 'with prob value', prob_value)
if same_prob:
print(
'-------------------------------> UNKNOWN PROBABILITY, STOP INFERENCE'
)
return eef_traj, False, 0
next_mov_discr = [next_mov_discr] ## to compute move later
except Exception as e:
if sim_param.debug_infer:
print(
'-------------------------------> UNKNOWN LABEL WHILE INFERRING!!!',
e)
return eef_traj, False, 0
else:
## compute neighbours virtual positions
nn_pos_vector = [[current_eef_x, current_eef_y, current_eef_z]]
add_dist = sim_param.step_length / 2
for j in range(sim_param.nb_NN):
tmp_pos = [current_eef_x, current_eef_y, current_eef_z]
if j == 0: ## front
tmp_pos[0] = current_eef_x + add_dist
elif j == 1: ## back
tmp_pos[0] = current_eef_x - add_dist
elif j == 2: ## right
tmp_pos[1] = current_eef_y - add_dist
elif j == 3: ## left
tmp_pos[1] = current_eef_y + add_dist
elif j == 4: ## up
tmp_pos[2] = current_eef_z + add_dist
elif j == 5: ## down
tmp_pos[2] = current_eef_z - add_dist
tmp_pos = [
round(curr_pos, sim_param.round_value)
for curr_pos in tmp_pos
]
# print(j, tmp_pos)
nn_pos_vector.append(tmp_pos)
## compute their prob for next move
for curr_virt_pos in nn_pos_vector:
current_eef_x_tmp = curr_virt_pos[0]
current_eef_y_tmp = curr_virt_pos[1]
current_eef_z_tmp = curr_virt_pos[2]
## compute current variables value
node_names = ['effect']
node_values = [expected_effect]
obj_id = 0
for obj_name in sim_param.obj_name_vector:
# print('obj pos inferring -->', obj_name, obj_pos_dict[obj_name])
distance = discr_dist.compute_distance(
[current_eef_x_tmp, current_eef_y_tmp],
obj_pos_dict[obj_name], current_dist)
orientation = discr_orien.compute_orientation_discr(
[current_eef_x_tmp, current_eef_y_tmp],
obj_pos_dict[obj_name], current_orien)
inclination = discr_inclin.compute_inclination_discr(
obj_pos_dict[obj_name], [
current_eef_x_tmp, current_eef_y_tmp,
current_eef_z_tmp
], current_inclin)
node_names += [
'distance' + str(obj_id), 'orientation' + str(obj_id),
'inclination' + str(obj_id)
]
node_values += [distance, orientation, inclination]
obj_id += 1
## infere next move
try:
next_mov_discr, prob_value, delta_nb_var, same_prob = \
infer.infere_mov(bn, ie,
node_names,
node_values)
if sim_param.debug_infer:
print(node_values, next_mov_discr, prob_value)
except Exception as e:
if sim_param.debug_infer:
print(
'-------------------------------> UNKNOWN LABEL WHILE INFERRING!!!',
e)
print([curr_virt_pos, node_values])
virt_inference_vector.append(
[curr_virt_pos, '', '', 0]) ## to avoid being selected
continue ## dont do next steps
if sim_param.debug_infer:
print([curr_virt_pos, node_values, next_mov_discr, prob_value])
virt_inference_vector.append(
[curr_virt_pos, node_values, next_mov_discr, prob_value])
## check if same prob for all close points
prob_found = all([
x[-1] == virt_inference_vector[0][-1]
for x in virt_inference_vector
])
if prob_found and len(
virt_inference_vector) > 2 and sim_param.debug_infer:
print('-------------------------------> SAME PROB',
virt_inference_vector[0][-1])
if virt_inference_vector[0][-1] == 0:
print('-------------------------------> NO MOVE INFERRED')
print(node_values)
return eef_traj, False
if sim_param.debug_infer:
print(node_values)
# ## MOVE
# ## with higher prob
# max_prob = virt_inference_vector[0][3]
# max_next_move = virt_inference_vector[0][2]
# max_pos = 0
# tmp_i = 1
# for tmp_infer_values in virt_inference_vector[1:]:
# if tmp_infer_values[3] > max_prob:
# max_prob = tmp_infer_values[3]
# max_next_move = tmp_infer_values[2]
# max_pos = tmp_i
# tmp_i += 1
# print('Next move for pos', max_next_move.upper(), max_prob, 'in pose', max_pos)
# next_mov_discr = [max_next_move]
## get 2 higher values
## if close, we select the one more repeated
## else, the higher value
cumulated_prob_dict = OrderedDict()
for tmp_infer_values in virt_inference_vector:
if not tmp_infer_values[2] in cumulated_prob_dict.keys():
cumulated_prob_dict[
tmp_infer_values[2]] = 0 ## cumulated prob
cumulated_prob_dict[tmp_infer_values[2]] += tmp_infer_values[3]
# counter_dict = Counter(cumulated_prob_dict) ## { key1:n times, key2:p times etc }
keys_found_list = []
for curr_res in virt_inference_vector:
keys_found_list.append(curr_res[2])
counter_dict = OrderedDict()
for key in keys_found_list:
if key == '':
continue
if not key in counter_dict:
counter_dict[key] = 1
else:
counter_dict[key] += 1
mean_prob_dict = OrderedDict()
for move, repetitions in counter_dict.items():
print(move, repetitions)
mean_prob_dict[move] = round(
cumulated_prob_dict[move] / repetitions,
sim_param.round_value) ## mean value
sorted_mean_list = sorted(mean_prob_dict.items(),
key=operator.itemgetter(1),
reverse=True)
print(sorted_mean_list)
max_next_move = ''
if len(sorted_mean_list) == 1 or sorted_mean_list[0][1] > 0.95:
max_next_move = sorted_mean_list[0][0]
max_prob = sorted_mean_list[0][1]
## if 2 elems check if are close
elif len(sorted_mean_list) > 1:
## if are close, get the more repeated one
if round(sorted_mean_list[1][1] / sorted_mean_list[0][1],
2) >= 0.75:
if counter_dict[sorted_mean_list[0][0]] >= \
counter_dict[sorted_mean_list[1][0]]:
max_next_move = sorted_mean_list[0][0]
max_prob = sorted_mean_list[0][1]
else:
max_next_move = sorted_mean_list[1][0]
max_prob = sorted_mean_list[1][1]
else:
max_next_move = sorted_mean_list[0][0]
max_prob = sorted_mean_list[0][1]
print('Next move for pos', max_next_move.upper(), 'with mean prob',
max_prob)
next_mov_discr = [max_next_move]
# if sim_param.debug_infer:
# print(expected_effect.upper(),
# orientation.upper(),
# inclination.upper(),
# distance.upper(),
# "-> ",
# next_mov_discr.upper(),
# "with probability", prob_value)
## COMMON CODE AGAIN !
## compute displacement regarding move or moves
# print('next_mov_discr', next_mov_discr)
delta_x = 0
delta_y = 0
delta_z = 0
for curr_move in next_mov_discr:
move_coord = 1 ## if move in this coord
if 'far' in curr_move:
delta_x = move_coord
elif 'close' in curr_move:
delta_x = -move_coord
if 'right' in curr_move:
delta_y = -move_coord
elif 'left' in curr_move:
delta_y = move_coord
if 'up' in curr_move:
delta_z = move_coord
elif 'down' in curr_move:
delta_z = -move_coord
## length of the movement must be equal to step_length
# dims_vector = abs(delta_x) + abs(delta_y) + abs(delta_z)
# if dims_vector == 0: ## to avoid div by 0 ??? TODOOOOOOOOOOOOOOOOOOOOOOO
# dims_vector = 1
dims_vector = 1
mov_step = round(sim_param.step_length / sqrt(dims_vector),
sim_param.round_value)
if delta_x > 0:
delta_x = mov_step
elif delta_x < 0:
delta_x = -mov_step
if delta_y > 0:
delta_y = mov_step
elif delta_y < 0:
delta_y = -mov_step
if delta_z > 0:
delta_z = mov_step
elif delta_z < 0:
delta_z = -mov_step
if sim_param.debug_infer:
print(
"Delta :", delta_x, delta_y, delta_z, 'Norm:',
round(d.euclidean([0, 0, 0], [delta_x, delta_y, delta_z]),
sim_param.round_value))
## move eef to new position
# next_eef_x = round(current_eef_x + delta_x/len(next_mov_discr), sim_param.round_value)
# next_eef_y = round(current_eef_y + delta_y/len(next_mov_discr), sim_param.round_value)
# next_eef_z = round(current_eef_z + delta_z/len(next_mov_discr), sim_param.round_value)
next_eef_x = round(current_eef_x + delta_x, sim_param.round_value)
next_eef_y = round(current_eef_y + delta_y, sim_param.round_value)
next_eef_z = current_eef_z
if sim_param.debug_infer:
print('Previous EEF pos :', current_eef_x, current_eef_y,
current_eef_z)
print('New EEF pos :', next_eef_x, next_eef_y, next_eef_z)
eef_traj.append([next_eef_x, next_eef_y, next_eef_z])
## check if obj was moved
obj_moved_pose_dict = OrderedDict()
for obj_name in sim_param.moved_obj_name_vector:
obj_pos = obj_pos_dict[obj_name]
obj_moved, obj_moved_pose = \
env.compute_obj_pos(
obj_pos,
[current_eef_x,current_eef_y,current_eef_z],
[next_eef_x,next_eef_y,next_eef_z])
obj_moved_pose_dict[obj_name] = (obj_moved, obj_pos)
all_obj_moved = True
for name, moved in obj_moved_pose_dict.items():
if sim_param.debug_infer:
print('simulated', name.upper(), "MOVED ? :", moved[0])
all_obj_moved = all_obj_moved and moved[0]
i += 1
## end delta
## repeat last move to improve effect identification
if all_obj_moved:
current_eef_x = next_eef_x
current_eef_y = next_eef_y
current_eef_z = next_eef_z
next_eef_x = round(current_eef_x + delta_x / len(next_mov_discr),
sim_param.round_value)
next_eef_y = round(current_eef_y + delta_y / len(next_mov_discr),
sim_param.round_value)
next_eef_z = round(current_eef_z + delta_z / len(next_mov_discr),
sim_param.round_value)
eef_traj.append([next_eef_x, next_eef_y, next_eef_z])
mean_traj_prob = mean_traj_prob / len(eef_traj)
return eef_traj, all_obj_moved, mean_traj_prob