def pick_and_place(env, pick_pose, pick_object_name, place_pose):
# print"\tPICK AND PLACE ACTION => rearrange", pick_object_name
pick_time = env.pick(env.obs_pos, pick_pose, place_pose)
CLF.att_box_client('hand', pick_object_name)
ready_time1 = env.go_ready()
place_time = env.place(env.obs_pos, place_pose)#, vrep_env.get_current_joint(joint_names_jaco))
CLF.det_box_client(pick_object_name, [0, 0, 0], [0, 0, 0, 0], [0, 0, 0], 'red')
CLF.add_mesh_client(pick_object_name, [place_pose[0], place_pose[1], 0.605], [0.0, 0.0, 0.0, 0.0], [0.001, 0.001, 0.001])
ready_time2 = env.go_ready()
# print"exe time:", pick_time + ready_time1 + place_time + ready_time2
return pick_time+ready_time1+place_time+ready_time2
def test_algorithm(method_in, data_in):
# method
# "where" : icra2020 "where to relocate?"
# "far" : farthest method
go_ready()
hand_open()
# print "start with method:", method_in
# print "\n***STEP 1*** : env setting"
obj_h = -0.0
obj_z = 0.605 + obj_h#+ obj_h/2.0
target_name = ['target']
target_info = []
if -data_in[0][0]>0:
target_info.append([[data_in[0][1] - 0.04, -data_in[0][0]-0.02, obj_z], [0, 0, 0, 0], [0.001, 0.001, 0.001]]) # for the add_mesh
else:
target_info.append([[data_in[0][1] - 0.04, -data_in[0][0]+0.02, obj_z], [0, 0, 0, 0], [0.001, 0.001, 0.001]]) # for the add_mesh
# target_info.append([[data_in[0][0], data_in[0][1], obj_z], [0, 0, 0, 0], [0.06, 0.06, 0.12]]) # for the add_box
# target_info[i][0][2] = target_info[i][0][2] + 0.04
# target_info[i][2][2] = target_info[i][2][2] + 0.08
# obstacle_name = []
# for i in range(len(data_in[1])):
obstacle_name = [str(i).zfill(2) for i in range(len(data_in[1]))]
# obstacle_name.append('obstacle'+str(i))
# print obstacle_name
# obstacle_name = ['obstacle0', 'obstacle1', 'obstacle2', 'obstacle3', 'obstacle4', 'obstacle5', 'obstacle6', 'obstacle7', 'obstacle8']
obstacle_info = []
# [[obj_pos.x, obj_pos.y, obj_pos.z], [obj_ori_q.x, obj_ori_q.y, obj_ori_q.z, obj_ori_q.w], [obj_scale.x, obj_scale.y, obj_scale.z]]
for i in range(len(obstacle_name)):
obstacle_info.append([[data_in[1][i][1]-0.02, -data_in[1][i][0], obj_z], [0, 0, 0, 0], [0.001, 0.001, 0.001]]) # for the add_mesh
# obstacle_info.append([[data_in[1][i][0], data_in[1][i][1], obj_z], [0, 0, 0, 0], [0.06, 0.06, 0.12]]) # for the add_box
# obstacle_info[i][0][2] = obstacle_info[i][0][2] + 0.04
# obstacle_info[i][2][2] = obstacle_info[i][2][2] + 0.08
can_r = 0.02
if len(obstacle_name)>17:
can_r = 0.035
# print "can r", len(obstacle_name), can_r
elif len(obstacle_name)>13:
can_r = 0.045
# print "can r", len(obstacle_name), can_r
elif len(obstacle_name)>10:
can_r = 0.045
# print "can r", len(obstacle_name), can_r
# print "\tNo. of obstacles:", len(obstacle_name)
env_name = ['shelf_gazebo']#2020.10.21: puzzle test, 'Jaco_base', 'table_ls', 'table_rs', 'table_us', 'table_bs']
env_info = []
base_position = [0.8637-0.02, 0, 0.0 + obj_h]
base_quaternion = [0, 0, 0, 1]
base_scale = [0.001, 0.001, 0.001]
CLF.add_mesh_client('shelf_gazebo', base_position, base_quaternion, base_scale)
ws_pos = [0.8637+0.5*0.45+0.03-0.02, 0.0, 0.0 + obj_h]
ws_rot = [0.0, 0.0, 0.0, 0.0]
ws_scale = [0.45, 0.91, 0.0]
env_info.append([ws_pos, ws_rot, ws_scale])
# for i in range(len(env_name)):
# env_info.append(vrep_env.get_object_info(env_name[i]))
# if i > 1:
# env_info[i][2][0] = env_info[i][2][0]+0.01
# env_info[i][2][1] = env_info[i][2][1]+0.01
# env_info[i][2][2] = env_info[i][2][2]+0.01
for i in range(len(obstacle_info)):
CLF.add_mesh_client(obstacle_name[i], obstacle_info[i][0], obstacle_info[i][1], obstacle_info[i][2])
# CLF.add_box_client(obstacle_name[i], obstacle_info[i][0], obstacle_info[i][1], obstacle_info[i][2], 'red')
for i in range(len(target_info)):
CLF.add_mesh_client(target_name[i], target_info[i][0], target_info[i][1], target_info[i][2])
# CLF.add_box_client(target_name[i], target_info[i][0], target_info[i][1], target_info[i][2], 'green')
# for i in range(len(env_info)):
# # CLF.add_mesh_client(env_name[i], env_info[i][0], env_info[i][1], env_info[i][2])
# CLF.add_box_client(env_name[i], env_info[i][0], env_info[i][1], env_info[i][2], 'gray')
ws = env_info[0]
# print"ws info", env_info[0]
ws_w = int(round(ws[2][0]*100)) # x-axes in Rviz
ws_d = int(round(ws[2][1]*100)) # y-axes in Rviz
# print "\tRviz ws width, depth:", ws_w, ws_d
# GRID_SIZE = 0.01
ws_zero_pos = [round(ws[0][0] - ws[2][0]*0.5, 2), round(ws[0][1] - ws[2][1]*0.5, 2)]
# print "\tRviz ws cen pos:", ws[0]
# print "\tRviz ws, zero pos:", ws_zero_pos
# ws_w, ws_d = 100, 100 # get table size in the v-rep
ws_cen = [-ws[0][1], ws[0][0]]
rob_pos = [0.0, 0.0]
OBJ_R = 0.035
env = EI(rob_pos, ws_w, ws_d, ws_cen, ws_zero_pos, grid_size=GRID_SIZE, wall_r=OBJ_R)
env.set_env(obstacle_name, obstacle_info, target_name, target_info)
env.update_env(env.obs_pos, env.obs_grid)
print "env.ore_order:", env.ore_order, env.ore_order[0]
# print "\trearrangement order:", env.ore_order
# if len(env.ore_order) == 0:
# print "end rearrangement"
# pick_and_place(env, env.tar_pos, 'target', env.tar_pos)
# time.sleep(1)
# CUF.draw_grid_info(env.grid_ori)
# plt.show()
ret = 0
space_err = 0
rearr_cnt = 0
reloc_cnt = len(env.ore_order)
# print"first reloc cnt:", reloc_cnt
# env.get_env(obs_r, tar_r, min_ore)
algorithm_start = time.time()
tp_time = 0
mp_time = 0
ex_time = 0
method = method_in
# method = 'mine'
# method = 'far'
# method = 'deep'
while len(env.ore_order): # this while loop is for the algorithm
# print"\n***STEP 2*** REARRANGE ORDER => :", env.ore_order
env.get_max_can(env.grid_ori, bt_num=2, trial_num=2500, can_r=can_r) # We get "grid_max_can", "can_grid"
# env.get_env_case1()
# env.get_max_can_case1()
'''
Make object info!
Type : target, obstacle, candidate
Info : pos, grid, A, BT, b, ORC, ORE
'''
can_info = []
for i in range(len(env.can_pos)):
can_info.append((CI('candidate', env.can_pos[i], env.can_grid[i])))
# check env info got right
# if 1:
# print "\n# of obstacles", len(env.obs_pos), "\n# of candidates", len(env.can_pos)
'''
GET candidates info
'''
t_ore_order = copy.deepcopy(env.ore_order)
# for i in range(len(can_info)):
# print "can", i, ":", can_info[i].pos
# CUF.draw_grid_info(env.grid_ori)
# CUF.draw_grid_info(env.grid_del)
# CUF.draw_grid_info(env.grid_max_can)
# for c_i in range(len(can_info)):
# plt.text(can_info[c_i].grid[0], can_info[c_i].grid[1], 'Can' + str(c_i), fontsize=20, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for o_i in range(len(env.obs_grid)):
# plt.text(env.obs_grid[o_i][0], env.obs_grid[o_i][1], 'Obs' + str(o_i), fontsize=20, ha='center', bbox=dict(facecolor='red', alpha=0.8))
# plt.show()
# print"\tCheck C.A"
# Check C.A : just next step
t_can_info = []
in_can_info = copy.deepcopy(can_info)
in_obs_pos = copy.deepcopy(env.obs_pos)
in_obs_pos.remove(env.obs_pos[env.ore_order[0]])
CLF.del_box_client(obstacle_name[env.ore_order[0]])
tmp_mp_start = time.time()
t_can_info.append(env.get_can_A(in_can_info, in_obs_pos, env.tar_pos))
tmp_mp_end = time.time()
mp_time = mp_time + tmp_mp_end - tmp_mp_start
print "in loop env.ore_order:", env.ore_order, env.ore_order[0]
CLF.add_mesh_client(obstacle_name[env.ore_order[0]], obstacle_info[env.ore_order[0]][0], obstacle_info[env.ore_order[0]][1], obstacle_info[env.ore_order[0]][2])
# CLF.add_box_client(obstacle_name[env.ore_order[0]], obstacle_info[env.ore_order[0]][0], obstacle_info[env.ore_order[0]][1], obstacle_info[env.ore_order[0]][2], 'red')
# Check C.BT
in_can_info = copy.deepcopy(t_can_info[0])
in_can_info = env.init_BT(in_can_info) # init the BT value of candidates to '0'
in_obs_pos = copy.deepcopy(env.obs_pos)
for ore_i in range(len(env.ore_order)): # after rearrange all ORE
in_obs_pos.remove(env.obs_pos[env.ore_order[ore_i]])
CLF.del_box_client(obstacle_name[env.ore_order[ore_i]])
tmp_mp_start = time.time()
t_can_info[0] = env.get_can_BT(in_can_info, in_obs_pos, env.tar_pos)
tmp_mp_end = time.time()
mp_time = mp_time + tmp_mp_end - tmp_mp_start
for ore_i in range(len(env.ore_order)):
CLF.add_mesh_client(obstacle_name[env.ore_order[ore_i]], obstacle_info[env.ore_order[ore_i]][0], obstacle_info[env.ore_order[ore_i]][1], obstacle_info[env.ore_order[ore_i]][2])
# CLF.add_box_client(obstacle_name[env.ore_order[ore_i]], obstacle_info[env.ore_order[ore_i]][0], obstacle_info[env.ore_order[ore_i]][1], obstacle_info[env.ore_order[ore_i]][2], 'red')
# Check C.BO : BO : other ORE, just before target
in_can_info = copy.deepcopy(t_can_info[0])
in_obs_pos = copy.deepcopy(env.obs_pos)
for ore_i in range(len(env.ore_order)): # after rearrange all ORE
in_obs_pos.remove(env.obs_pos[env.ore_order[ore_i]])
CLF.del_box_client(obstacle_name[env.ore_order[ore_i]])
for j in range(len(env.ore_order)): # check other ORE just before target
if j > i:
tmp_mp_start = time.time()
t_can_info[0] = env.get_can_BT(in_can_info, in_obs_pos, env.obs_pos[env.ore_order[j]])
tmp_mp_end = time.time()
mp_time = mp_time + tmp_mp_end - tmp_mp_start
for ore_i in range(len(env.ore_order)):
CLF.add_mesh_client(obstacle_name[env.ore_order[ore_i]], obstacle_info[env.ore_order[ore_i]][0], obstacle_info[env.ore_order[ore_i]][1], obstacle_info[env.ore_order[ore_i]][2])
# CLF.add_box_client(obstacle_name[env.ore_order[ore_i]], obstacle_info[env.ore_order[ore_i]][0], obstacle_info[env.ore_order[ore_i]][1], obstacle_info[env.ore_order[ore_i]][2], 'red')
s_v = []
s_v_index = []
for i in range(1):
in_can_info = copy.deepcopy(t_can_info[i])
ret_can, ret_index = env.get_cf(in_can_info)
s_v.append(ret_can)
s_v_index.append(ret_index)
# print "\n step", i, " has # of cf pos:", len(t_cf[i]), "index", t_cf_index[i]
# print"\n***STEP 3*** : find valid candidates"
# print "\ts_v:", len(s_v[0]), "\n\ts_v_index:", len(s_v_index[0])
# for i in range(len(s_v[0])):
# print "s_v index:", [i], s_v_index[0][i]
# See the feasibile candidate
# for i in range(len(t_cf[0])):
# print "\n Our Cf pos:", i, t_cf[0][i].pos
# See if this case if case0 or case1
# print "t_cf:", t_cf, "order", env.ore_order
# if len(s_v[0]) >= len(env.ore_order):
if len(s_v[0]) >= 1:
# print "\n\tenough candidate spots"
t_b = []
for i in range(1):
in_obs_pos = copy.deepcopy(env.obs_pos)
for ore_i in range(i + 1):
in_obs_pos.remove(env.obs_pos[env.ore_order[ore_i]])
t_b.append(env.get_cf_b(s_v[i], in_obs_pos))
# print "\n step", i, " has cf b:", t_b[i]
# draw_figs = 1
# if draw_figs == 1:
# for c_i in range(len(can_info)):
# plt.text(can_info[c_i].grid[0], can_info[c_i].grid[1], 'Can' + str(c_i), fontsize=20, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for o_i in range(len(env.obs_grid)):
# plt.text(env.obs_grid[o_i][0], env.obs_grid[o_i][1], 'Obs' + str(o_i), fontsize=20, ha='center', bbox=dict(facecolor='red', alpha=0.8))
#
# for step_i in range(1):
# step_grid = copy.deepcopy(env.grid_act)
# step_obs_grid = copy.deepcopy(env.obs_grid)
# for ore_i in range(step_i + 1):
# step_obs_grid.remove(env.obs_grid[env.ore_order[ore_i]])
# for i in range(len(step_obs_grid)):
# step_grid = CUF.obstacle_circle(step_grid, [round(step_obs_grid[i][0], 2), round(step_obs_grid[i][1], 2), env.obs_r[i]], 2)
# for ci in range(len(can_info)):
# xi, yi = can_info[ci].grid
# step_grid = CUF.obstacle_circle(step_grid, [xi, yi, 0.04], 30)
#
# step_grid = CUF.obstacle_circle(step_grid, [env.tar_grid[0], env.tar_grid[1], tar_r], 4) # target
#
# for cf_i in range(len(t_b[step_i])):
# xi = (t_cf[step_i][cf_i].pos[0] - env.ws_zero[0]) * G2P_SIZE
# yi = (t_cf[step_i][cf_i].pos[1] - env.ws_zero[1]) * G2P_SIZE
# step_grid = CUF.obstacle_circle(step_grid, [xi, yi, 0.04], 3)
#
# CUF.draw_grid_info(step_grid)
#
# for cf_i in range(len(t_b[step_i])):
# xi = (t_cf[step_i][cf_i].pos[0] - env.ws_zero[0]) * G2P_SIZE
# yi = (t_cf[step_i][cf_i].pos[1] - env.ws_zero[1]) * G2P_SIZE
# plt.text(xi, yi, 'b=' + str(t_b[step_i][cf_i]), fontsize=20, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for ci in range(len(t_can_info[step_i])):
# plt.text(t_can_info[step_i][ci].grid[0], t_can_info[step_i][ci].grid[1] - 2.0, '[A, BT] :' + str([t_can_info[step_i][ci].A, t_can_info[step_i][ci].BT]), fontsize=10, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for o_i in range(len(env.obs_grid)):
# plt.text(env.obs_grid[o_i][0], env.obs_grid[o_i][1], 'Obs' + str(o_i), fontsize=20, ha='center', bbox=dict(facecolor='red', alpha=0.8))
# plt.title('step' + str(step_i) + " obs: " + str(env.ore_order[step_i]) + " rearranged")
elif len(s_v[0]) < len(env.ore_order):
# print "\n\tnot enough candidate spots"
# print "Since we meet condition: N(CF) < N(ORE) by", len(t_cf[0]), "<", len(env.ore_order), ",\nwe have to remove additional obstacles."
## step1 : "get t_cp", check candidates which have A = 0 and BT = 0
## This means that a candidate is not reachable and it does not block the target object
# Check A for this environment state
in_can_info = copy.deepcopy(can_info)
in_obs_pos = copy.deepcopy(env.obs_pos)
t_can_add = copy.deepcopy(env.get_can_A(in_can_info, in_obs_pos, env.tar_pos))
s_e = [] # s_e: extra candidate spots
in_can_info = copy.deepcopy(t_can_add)
ret_can, ret_index = env.get_cp(in_can_info)
# print "\t# of OR'", len(ret_can)
t_s_e = ret_can
t_s_e_index = ret_index
# print "t_cp:", len(t_cp), "index", t_cp_index
# for i in range(len(t_cp)):
# print "\n Our Cp:", i, t_cp[i].pos
if len(t_s_e) == 0:
# print "\tno possible extra candidate exist"
space_err = 1
break
# step2 : check c_ore for each cp and pick min of it
t_s_r = [] # s_r: candidate spot relocate plan
in_can_info = copy.deepcopy(t_s_e)
# tmp_order_time_start = timeit.default_timer()
# tmp_order_time_start2 = time.clock()
t_s_r = env.get_c_ore(in_can_info)
# tmp_order_time_end = timeit.default_timer()
# tmp_order_time_end2 = time.clock()
# order_time = order_time + tmp_order_time_end - tmp_order_time_start
# order_time2 = order_time2 + tmp_order_time_end2 - tmp_order_time_start2
# order_cnt = order_cnt + 100 * len(t_s_e)
# print "\n"
# for i in range(len(t_cp)):
# print "cp", t_cp[i].pos, "\nc_ore", c_ore[i]
s_r = []
s_e_index = []
# print "\n"
# for i in range(len(t_s_e)):
# print "can", t_s_e_index[i], "grid:", t_s_e[i].grid, ", s_r:", t_s_r[i]
for i in range(len(t_s_e)):
if t_s_r[i] != []:
s_e.append(t_s_e[i])
s_r.append(t_s_r[i])
s_e_index.append(t_s_e_index[i])
# tmp_se = copy.deepcopy(s_e)
# tmp_sr = copy.deepcopy(s_r)
# emp_sr = []
# for i in range(len(s_e)):
# if s_r[i] == []:
# print "remove empty s_e", i
# emp_sr.append(i)
#
# print "tmp se:", tmp_se, "\ntmp sr", tmp_sr
# for i in range(len(emp_sr)):
#
# print "tmp_se[emp_sr[i]]", tmp_se[emp_sr[i]].pos
# print "tmp_sr[emp_sr[i]]", tmp_sr[emp_sr[i]]
# s_e.remove(tmp_se[emp_sr[i]])
# s_r.remove(tmp_sr[emp_sr[i]])
while len(s_e):
# print "# of s_e:", len(s_e), s_r
# print "\n"
# for i in range(len(s_e)):
# print "can", s_e_index[i], "pos:", s_e[i].pos, ", s_r:", s_r[i]
min_s_r = CUF.min_len_list(s_r)
# print "\nmin sr:", min_s_r
#
# print "picked ci index:", t_cp.index(t_cp[c_ore.index(min_c_ore)])
# print "picked ci address:", copy.deepcopy(t_cp[c_ore.index(min_c_ore)]).pos
cp = copy.deepcopy(s_e[s_r.index(min_s_r)])
# print "selected cp pos", cp.pos
## step3 : "get t_cf", check candidates which have A = 1 and BT' = 0
## Check A for this environment state T' is t_cp_i
in_can_info = copy.deepcopy(can_info)
in_obs_pos = copy.deepcopy(env.obs_pos)
in_tar_pos = copy.deepcopy(cp.pos)
# t_can_add = copy.deepcopy(env.get_can_A(in_can_info, in_obs_pos, in_tar_pos))
t_can_add = copy.deepcopy(env.get_can_A(in_can_info, in_obs_pos, env.tar_pos))
# Check C.BT for this environment state
in_can_info = copy.deepcopy(t_can_add)
in_can_info = env.init_BT(in_can_info) # init the BT value of candidates to '0'
in_obs_pos = copy.deepcopy(env.obs_pos)
sorted_min_s_r = copy.deepcopy(min_s_r)
sorted_min_s_r.sort(reverse=True)
# print "sorted min_s_r:", sorted_min_s_r
if sorted_min_s_r[0] == len(env.obs_pos): # if OR' has o_t ! remove s_e
# print "o_t is in OR'"
s_e.remove(s_e[s_r.index(min_s_r)])
s_e_index.remove(s_e_index[s_r.index(min_s_r)])
s_r.remove(s_r[s_r.index(min_s_r)])
else:
for ore_i in range(len(min_s_r)): # after rearrange all OR'
in_obs_pos.remove(in_obs_pos[sorted_min_s_r[ore_i]])
CLF.del_box_client(obstacle_name[sorted_min_s_r[ore_i]])
in_tar_pos = copy.deepcopy([-cp.pos[1], cp.pos[0]])
t_can_add = env.get_can_BT(in_can_info, in_obs_pos, in_tar_pos)
for ore_i in range(len(min_s_r)): # after rearrange all OR'
CLF.add_mesh_client(obstacle_name[sorted_min_s_r[ore_i]], obstacle_info[sorted_min_s_r[ore_i]][0], obstacle_info[sorted_min_s_r[ore_i]][1], obstacle_info[sorted_min_s_r[ore_i]][2])
# CLF.add_box_client(obstacle_name[sorted_min_s_r[ore_i]], obstacle_info[sorted_min_s_r[ore_i]][0], obstacle_info[sorted_min_s_r[ore_i]][1], obstacle_info[sorted_min_s_r[ore_i]][2], 'red')
# for i in range(len(t_can_add)):
# print "can", i, "A:", t_can_add[i].A, "B:", t_can_add[i].BT
s_e_v = []
s_v_index = []
in_can_info = copy.deepcopy(t_can_add)
ret_can, ret_index = env.get_cf(in_can_info)
s_e_v.append(ret_can)
s_v_index.append(ret_index)
# print "s_e_v: ", s_e_v
# for i in range(len(s_e_v[0])):
# print s_e_v[0][i].grid
if len(s_e_v[0]) >= len(min_s_r) - 1:
# print "this se is possible"
if len(min_s_r) == 1:
# print "only one move needed"
# t_can_info = []
# for i in range(len(env.ore_order)):
# in_can_info = copy.deepcopy(can_info)
# in_obs_pos = copy.deepcopy(env.obs_pos)
# for ore_i in range(i + 1):
# if min_s_r[0] != env.ore_order[ore_i]:
# in_obs_pos.remove(env.obs_pos[env.ore_order[ore_i]])
# in_obs_pos.remove(env.obs_pos[min_s_r[0]])
# t_can_info.append(env.get_can_A(in_can_info, in_obs_pos, env.tar_pos))
s_v = [[s_e[s_r.index(min_s_r)]]]
s_v_index = [[s_e_index[s_r.index(min_s_r)]]]
# print "se v:", s_v, s_v[0], s_v[0][0], s_v[0][0].pos
# for i in range(len(env.ore_order)):
# add_can_info = copy.deepcopy(t_can_info[i])
# ret_can, ret_index = env.get_cf(add_can_info)
# s_v.append(ret_can)
# s_v_index.append(ret_index)
t_b = [[0]]
# for i in range(1):
# in_obs_pos = copy.deepcopy(env.obs_pos)
# for ore_i in range(i+1):
# in_obs_pos.remove(env.obs_pos[env.ore_order[ore_i]])
# t_b.append(env.get_cf_b(s_v[i], in_obs_pos))
# # print "\n step", i, " has cf b:", t_b[i]
break # for out s_e loop
else:
t_b = []
in_obs_pos = copy.deepcopy(env.obs_pos)
for ore_i in range(1):
in_obs_pos.remove(env.obs_pos[min_s_r[ore_i]])
t_b.append(env.get_cf_b(s_e_v[0], in_obs_pos))
s_v[0] = s_e_v[0]
break # for out s_e loop
else: # s_e[s_r.index(min_s_r)]
# print "\nremove",
# print "s_e:", s_e
# print "s_r:", s_r
# print "s_e_index:", s_e_index
s_e.remove(s_e[s_r.index(min_s_r)])
s_e_index.remove(s_e_index[s_r.index(min_s_r)])
s_r.remove(s_r[s_r.index(min_s_r)])
if len(s_e) == 0:
# print "no possible extra candidate exist"
break
env.ore_order = min_s_r
# draw_figs = 1
# if draw_figs == 1:
# for c_i in range(len(can_info)):
# plt.text(can_info[c_i].grid[0], can_info[c_i].grid[1], 'Can' + str(c_i), fontsize=20, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for o_i in range(len(env.obs_grid)):
# plt.text(env.obs_grid[o_i][0], env.obs_grid[o_i][1], 'Obs' + str(o_i), fontsize=20, ha='center', bbox=dict(facecolor='red', alpha=0.8))
#
# step_i = 0
# step_grid = copy.deepcopy(env.grid_act)
# step_obs_grid = copy.deepcopy(env.obs_grid)
# step_obs_grid.remove(env.obs_grid[env.ore_order[0]])
# for i in range(len(step_obs_grid)):
# # print "i:", i, "step_obs_grid [i]:", step_obs_grid[i]
# step_grid = CUF.obstacle_circle(step_grid, [round(step_obs_grid[i][0], 2), round(step_obs_grid[i][1], 2), env.obs_r[i]], 2)
# for ci in range(len(can_info)):
# xi, yi = can_info[ci].grid
# step_grid = CUF.obstacle_circle(step_grid, [xi, yi, 0.04], 30)
#
# step_grid = CUF.obstacle_circle(step_grid, [env.tar_grid[0], env.tar_grid[1], tar_r], 4) # target
#
# for cf_i in range(len(t_b[step_i])):
# xi = (t_cf[step_i][cf_i].pos[0] - env.ws_zero[0]) * G2P_SIZE
# yi = (t_cf[step_i][cf_i].pos[1] - env.ws_zero[1]) * G2P_SIZE
# step_grid = CUF.obstacle_circle(step_grid, [xi, yi, 0.04], 3)
#
# CUF.draw_grid_info(step_grid)
#
# for cf_i in range(len(t_b[step_i])):
# xi = (t_cf[step_i][cf_i].pos[0] - env.ws_zero[0]) * G2P_SIZE
# yi = (t_cf[step_i][cf_i].pos[1] - env.ws_zero[1]) * G2P_SIZE
# plt.text(xi, yi, 'b=' + str(t_b[step_i][cf_i]), fontsize=20, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for ci in range(len(t_can_info[step_i])):
# plt.text(t_can_info[step_i][ci].grid[0], t_can_info[step_i][ci].grid[1] - 2.0, '[A, BT] :' + str([t_can_info[step_i][ci].A, t_can_info[step_i][ci].BT]), fontsize=10, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for o_i in range(len(env.obs_grid)):
# plt.text(env.obs_grid[o_i][0], env.obs_grid[o_i][1], 'Obs' + str(o_i), fontsize=20, ha='center', bbox=dict(facecolor='red', alpha=0.8))
# plt.title('step' + str(step_i) + " obs: " + str(env.ore_order[step_i]) + " rearranged")
if space_err:
# print "no possible extra candidate exist"
break
# move obstacle to can(min(b))
# print "s_v", s_v
# print "s_v[0]", s_v[0]
# print "s_v[0][0]", s_v[0][0]
# print "s_v[0][0].pos", s_v[0][0].pos
# print "\tt_b[0]", t_b[0]
find_b = copy.deepcopy(t_b[0])
# print "move to c_", find_b.index(min(find_b))
if method == 'far':
t_sel_can_index = [i for i in range(len(find_b))]
elif method == 'deep':
t_sel_can_index = [i for i in range(len(find_b))]
elif method == 'SH_re':
t_sel_can_index = [i for i in range(len(find_b)) if find_b[i] == min(find_b)]
t_sel_can_dist = []
# print "\ntar grid: ", env.tar_grid
# print "\ntar pos: ", env.tar_pos
# print "\tt sel can index", t_sel_can_index
for i in range(len(t_sel_can_index)):
# print "t_cf grid x,y:", t_sel_can_index[i], t_cf[0][t_sel_can_index[i]].grid[0], t_cf[0][t_sel_can_index[i]].grid[1]
# print "t_cf pos x,y:", t_sel_can_index[i], s_v[0][t_sel_can_index[i]].pos[0], s_v[0][t_sel_can_index[i]].pos[1]
if method == 'deep':
t_sel_can_dist.append(np.sqrt((env.rob_pos[1] - s_v[0][t_sel_can_index[i]].pos[0]) ** 2 + (-env.rob_pos[0] - s_v[0][t_sel_can_index[i]].pos[1]) ** 2))
else:
t_sel_can_dist.append(np.sqrt((env.tar_pos[1] - s_v[0][t_sel_can_index[i]].pos[0]) ** 2 + (-env.tar_pos[0] - s_v[0][t_sel_can_index[i]].pos[1]) ** 2))
# print "obs pos:", env.obs_pos
# print "t sel can dist", t_sel_can_dist
sel_can_index = t_sel_can_index[t_sel_can_dist.index(max(t_sel_can_dist))]
# print "sel can index", sel_can_index
sel_can_pos = can_info[s_v_index[0][sel_can_index]].pos
# sel_can_pos = [can_info[s_v_index[0][sel_can_index]].pos[1], -can_info[s_v_index[0][sel_can_index]].pos[0]]
sel_can_grid = can_info[s_v_index[0][sel_can_index]].grid
# print"\npick obj in RVIZ:", env.obs_pos[env.ore_order[0]][1], -env.obs_pos[env.ore_order[0]][0]
# print"pick obj in ALG :", env.obs_pos[env.ore_order[0]]
sel_obs_pos = env.obs_pos[env.ore_order[0]]
sel_obs_grid = env.obs_grid[env.ore_order[0]]
# print"obstacle pos change:", [-can_info[s_v_index[0][sel_can_index]].pos[1], -can_info[s_v_index[0][sel_can_index]].pos[0]]
env.obs_pos[env.ore_order[0]] = [-can_info[s_v_index[0][sel_can_index]].pos[1], can_info[s_v_index[0][sel_can_index]].pos[0]]
env.obs_grid[env.ore_order[0]] = sel_can_grid
# print"place obj in RVIZ:", sel_can_pos
# print"place obj in ALG :", env.obs_pos[env.ore_order[0]]
# can_info[s_v_index[0][sel_can_index]].pos = sel_obs_pos
# can_info[s_v_index[0][sel_can_index]].grid = sel_obs_grid
# tmp_order_time_start = timeit.default_timer()
# tmp_order_time_start2 = time.clock()
# env.pick_n_place()
# CLF.add_box_client(obstacle_name[env.ore_order[0]], [env.object_z, -sel_can_pos[1], sel_can_pos[0]], obstacle_info[env.ore_order[0]][1], obstacle_info[env.ore_order[0]][2], 'blue')
# pick_and_place(env, sel_obs_pos, obstacle_name[env.ore_order[0]], env.obs_pos[env.ore_order[0]])
# print "obs pos:", env.obs_pos
tmp_ex_time = pick_and_place(env, sel_obs_pos, obstacle_name[env.ore_order[0]], sel_can_pos)
ex_time = ex_time + tmp_ex_time
obstacle_info[env.ore_order[0]][0] = [can_info[s_v_index[0][sel_can_index]].pos[0], can_info[s_v_index[0][sel_can_index]].pos[1], 0.605]
env.obs_pos[env.ore_order[0]] = copy.deepcopy([-can_info[s_v_index[0][sel_can_index]].pos[1], can_info[s_v_index[0][sel_can_index]].pos[0]])
env.obs_grid[env.ore_order[0]] = sel_can_grid
# def pick_and_place(env, pick_pose, pick_object_name, place_pose):
# print"\tPICK AND PLACE ACTION => rearrange", pick_object_name
#
# env.pick(env.obs_pos, pick_pose, place_pose)
# CLF.att_box_client('hand', pick_object_name)
# env.go_ready()
# env.place(env.obs_pos, place_pose) # , vrep_env.get_current_joint(joint_names_jaco))
# CLF.det_box_client(pick_object_name, [0, 0, 0], [0, 0, 0, 0], [0, 0, 0], 'red')
# CLF.add_mesh_client(pick_object_name, [place_pose[1], -place_pose[0], 0.605], [0.0, 0.0, 0.0, 0.0], [0.001, 0.001, 0.001])
# env.go_ready()
# print"\tEND PICK AND PLACE ACTION"
#
# time.sleep(1)
# obstacle_info = []
# for i in range(len(obstacle_name)):
# obstacle_info.append(vrep_env.get_object_info(obstacle_name[i]))
# # obstacle_info[i][0][2] = obstacle_info[i][0][2] + 0.04
# # obstacle_info[i][2][2] = obstacle_info[i][2][2] + 0.08
# for i in range(len(obstacle_info)):
# CLF.add_box_client(obstacle_name[i], obstacle_info[i][0], obstacle_info[i][1], obstacle_info[i][2], 'red')
# env.set_env(obstacle_name, obstacle_info, target_name, target_info)
# home_joint = [3.1415927410125732, 4.537856101989746, 5.93411922454834, -0.6108652353286743, 1.7453292608261108, -0.5235987901687622]
#
# CLF.move_joints_client_rad('arm', home_joint)
if len(env.ore_order) == 1:
rearr_cnt = rearr_cnt + 1
# print "end rearrangement"
# pick_and_place(env, env.tar_pos, 'target', env.tar_pos)
# time.sleep(1)
# plt.title('rearrangement finished')
# if rearr_cnt > 7:
print "rearr_cnt:", rearr_cnt
break
ret = env.update_env(env.obs_pos, env.obs_grid)
print "after update env -> env.ore_order:", env.ore_order#, env.ore_order[0]
if len(env.ore_order) > 14:
ret = -99
# rearr_cnt = rearr_cnt + 1
# print "end rearrangement"
# pick_and_place(env, env.tar_pos, 'target', env.tar_pos)
# time.sleep(1)
# plt.title('rearrangement finished')
break
# print "obs pos:", env.obs_pos
if len(env.ore_order) == 0:
rearr_cnt = rearr_cnt + 1
# print "end rearrangement"
# pick_and_place(env, env.tar_pos, 'target', env.tar_pos)
# time.sleep(1)
# plt.title('rearrangement finished')
break
if env.ore_order[0] == -1:
ret = -99
# print "no path"
break
# print "obs pos:", env.obs_pos
# tmp_order_time_end = timeit.default_timer()
# order_time = order_time + tmp_order_time_end - tmp_order_time_start
# order_time2 = order_time2 + tmp_order_time_end2 - tmp_order_time_start2
# order_cnt = order_cnt + 1
rearr_cnt = rearr_cnt + 1
if env.order_error_flag == 0:
# print "\nretry for another environment"
space_err = 1
break
# print "after move order is:", env.ore_order
# CUF.draw_grid_info(env.grid_ori)
# for c_i in range(len(can_info)):
# plt.text(can_info[c_i].grid[0], can_info[c_i].grid[1], 'Can' + str(c_i), fontsize=20, ha='center', bbox=dict(facecolor='pink', alpha=0.8))
# for o_i in range(len(env.obs_grid)):
# plt.text(env.obs_grid[o_i][0], env.obs_grid[o_i][1], 'Obs' + str(o_i), fontsize=20, ha='center', bbox=dict(facecolor='red', alpha=0.8))
if len(env.ore_order) == 0:
# print "end rearrangement"
# pick_and_place(env, env.tar_pos, 'target', env.tar_pos)
# time.sleep(1)
# plt.title('rearrangement finished')
break
# else:
# plt.title('after rearrngement')
# plt.show()
# pick_and_place(env, env.tar_pos, 'target', env.tar_pos)
# time.sleep(1)
if ret == -99:
return -99, -99, -99, -99, -99, -99
algorithm_end = time.time()#it.default_timer()
tot_time = algorithm_end - algorithm_start
tp_time = tot_time - mp_time - ex_time
print "n-rearrangement", reloc_cnt, rearr_cnt
print "tot, tp, mp, ex time:", tot_time, tp_time, mp_time, ex_time, "\n"
return reloc_cnt, rearr_cnt, tot_time, tp_time, mp_time, ex_time