def get_obs_wall(self, OBJ_R):
ws_side = []
ws_side.append([
self.ws_cen[0] - self.ws_w * self.GRID_SIZE * 0.5,
self.ws_cen[1] - self.ws_d * self.GRID_SIZE * 0.5 - OBJ_R
]) # left low point
ws_side.append([
self.ws_cen[0] + self.ws_w * self.GRID_SIZE * 0.5 + OBJ_R,
self.ws_cen[1] - self.ws_d * self.GRID_SIZE * 0.5 - OBJ_R
]) # right low point
ws_side.append([
self.ws_cen[0] + self.ws_w * self.GRID_SIZE * 0.5 + OBJ_R,
self.ws_cen[1] + self.ws_d * self.GRID_SIZE * 0.5 + OBJ_R
]) # right high point
ws_side.append([
self.ws_cen[0] - self.ws_w * self.GRID_SIZE * 0.5,
self.ws_cen[1] + self.ws_d * self.GRID_SIZE * 0.5 + OBJ_R
]) # left high point
obs_wall = []
obs_wall.extend(
CUF.linspace2D(ws_side[0], ws_side[1],
round(self.ws_w * self.GRID_SIZE / OBJ_R)))
obs_wall.extend(
CUF.linspace2D(ws_side[1], ws_side[2],
round(self.ws_d * self.GRID_SIZE / OBJ_R)))
obs_wall.extend(
CUF.linspace2D(ws_side[2], ws_side[3],
round(self.ws_w * self.GRID_SIZE / OBJ_R)))
return obs_wall
def get_max_can_case1(self):
# [19, 35], [24, 25]
self.can_grid = [[30, 70], [30, 50], [14, 25], [30, 10]]
self.can_pos = []
circle_r = max(self.ore_r) + 0.005
for i in self.can_grid:
xi, yi = i
self.can_pos.append([
self.ws_zero[0] + xi * self.GRID_SIZE,
self.ws_zero[1] + yi * self.GRID_SIZE
])
self.grid_max_can = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_max_can = CUF.obstacle_circle(self.grid_max_can, [
round(self.obs_grid[i][0], 2),
round(self.obs_grid[i][1], 2), self.obs_r[i]
], 2)
self.grid_max_can = CUF.obstacle_circle(
self.grid_max_can,
[self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
for i in range(len(self.can_grid)):
self.grid_max_can = CUF.obstacle_circle(
self.grid_max_can,
[self.can_grid[i][0], self.can_grid[i][1], circle_r], 3)
def update_env(self, in_obs_pos, in_obs_grid):
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(in_obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
self.ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall, self.rob_pos, self.rob_pos, self.d_max)
# check if the obstacles are rearranged then target is reachable
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_ore_pos = []
for i in self.ore_order:
if i != 'T':
tm_obs_pos.remove(self.obs_pos[i])
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.obs_wall,self.rob_pos, self.rob_pos, self.d_max)
# print "after removing:", tmp_order
# if tmp_order[0] == 'T':
# print "environment setting OK"
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(in_obs_grid)
self.obs_re_pos = copy.deepcopy(in_obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [round(self.obs_re_grid[i][0], 2), round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]], 2)
self.grid_del = CUF.obstacle_circle(self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
def update_env(self, in_obs_pos, in_obs_grid):
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(in_obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
# self.ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos,
tm_obs_ori, self.obs_wall, self.rob_pos,
self.rob_pos, self.d_max)
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(in_obs_grid)
self.obs_re_pos = copy.deepcopy(in_obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [
round(self.obs_grid[i][0], 2),
round(self.obs_grid[i][1], 2), self.obs_r[i]
], 2)
self.grid_ori = CUF.obstacle_circle(
self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r],
4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [
round(self.obs_re_grid[i][0], 2),
round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]
], 2)
self.grid_del = CUF.obstacle_circle(
self.grid_del, [self.tar_grid[0], self.tar_grid[1], self.tar_r],
4) # target
self.ore_order.pop()
def get_max_can(
self,
input_grid,
bt_num,
trial_num,
):
bt_circle = []
can_grid = []
circle_r = max(self.ore_r) + 0.005
for bt in range(bt_num):
grid_can = copy.deepcopy(
input_grid) # get original scene from the grid_set
empt_grid, occu_grid = CUF.getEmpOcc(grid_can)
for i in range(trial_num):
pick_cen = np.random.randint(0, len(empt_grid))
check_sum = 0
for oc in range(len(occu_grid)):
d_w = empt_grid[pick_cen][0] - occu_grid[oc][0]
d_d = empt_grid[pick_cen][1] - occu_grid[oc][1]
d_c = (d_w * d_w + d_d * d_d)**0.5 * self.GRID_SIZE
if d_c <= circle_r:
check_sum = 1
if check_sum == 0:
can_grid.append(empt_grid[pick_cen])
for em in range(len(empt_grid)):
d_w = empt_grid[pick_cen][0] - empt_grid[em][0]
d_d = empt_grid[pick_cen][1] - empt_grid[em][1]
d_c = (d_w * d_w + d_d * d_d)**0.5 * self.GRID_SIZE
if d_c <= circle_r:
grid_can[empt_grid[em][0]][empt_grid[em][1]] = 3
grid_can[empt_grid[pick_cen][0]][
empt_grid[pick_cen][1]] = 3
occu_grid.append(
[empt_grid[em][0], empt_grid[em][1]])
bt_circle.append([can_grid, grid_can])
max_cir_num = []
for i in range(len(bt_circle)):
max_cir_num.append([len(bt_circle[i][0])])
print(max_cir_num.index(max(max_cir_num)))
max_trial = max_cir_num.index(max(max_cir_num))
self.grid_max_can = copy.deepcopy(bt_circle[max_trial][1])
self.can_grid = bt_circle[max_trial][0]
self.can_pos = []
for i in self.can_grid:
xi, yi = i
self.can_pos.append([
self.ws_zero[0] + xi * self.GRID_SIZE,
self.ws_zero[1] + yi * self.GRID_SIZE
])
def __init__(self, rob_pos, ws_width, ws_depth, ws_cen, grid_size, wall_r):
self.rob_pos = rob_pos
self.GRID_SIZE = grid_size
self.ws_w = ws_width
self.ws_d = ws_depth
self.ws_cen = ws_cen
self.ws_zero = [round(self.ws_cen[0] - ws_width * self.GRID_SIZE * 0.5, 2), round(self.ws_cen[1] - ws_depth * self.GRID_SIZE * 0.5, 2)]
self.obs_wall = self.get_obs_wall(OBJ_R=wall_r)
self.d_max = 2.0
self.eta = 45
grid_act = np.zeros([ws_width, ws_depth])
self.grid_act = CUF.mark_edge_grid(grid_act)
def get_env(self, obs_r, tar_r, min_ore):
while 1:
self.obs_grid = []
grid_tmp = copy.deepcopy(self.grid_act)
self.obs_r = obs_r
self.tar_r = tar_r
for ri in self.obs_r:
grid_tmp, obs_center_tmp = CUF.place_circle_object_ig(
grid_tmp, ri, 2)
self.obs_grid.append(obs_center_tmp)
grid_tmp, tar_tmp = CUF.place_circle_object_ig(
grid_tmp, self.tar_r, 4)
self.tar_grid = copy.deepcopy(tar_tmp)
self.obs_pos = []
for i in self.obs_grid:
xi, yi = i
self.obs_pos.append([
round(xi * self.GRID_SIZE + self.ws_zero[0], 2),
round(yi * self.GRID_SIZE + self.ws_zero[1], 2)
])
self.tar_pos = [
round(self.tar_grid[0] * self.GRID_SIZE + self.ws_zero[0], 2),
round(self.tar_grid[1] * self.GRID_SIZE + self.ws_zero[1], 2)
] # target object!
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(self.obs_pos)
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
# ore_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
# ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos,
tm_obs_ori, self.obs_wall, self.rob_pos,
self.rob_pos, self.d_max)
# TM_noplot()
if len(ore_order) > min_ore:
print "environment setting OK"
print "target", self.tar_pos
print "obstacles", self.obs_pos
print "remove", ore_order, "th obstacles"
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
# tm_obs_pos.extend(self.obs_wall)
# ob = len(tm_obs_pos)
self.ore_order = ore_order
# self.ore_order = TM_plot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos, tm_obs_ori, self.rob_pos, self.rob_pos, self.d_max)
# check if the obstacles are rearranged then target is reachable
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_ore_pos = []
for i in self.ore_order:
if i != 'T':
tm_ore_pos.append(self.obs_pos[i])
for i in self.ore_order:
if i != 'T':
tm_obs_pos.remove(self.obs_pos[i])
# tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
# tmp_order = NG_ore(tm_tar_pos, tm_obs_pos, self.tar_r, self.obs_r, self.rob_pos, self.ws_zero, [self.ws_w * self.GRID_SIZE, self.ws_d * self.GRID_SIZE])
tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos,
tm_obs_ori, self.obs_wall, self.rob_pos,
self.rob_pos, self.d_max)
if len(tmp_order) == 1:
break
'''
with out additional rearrangement
'''
if len(self.ore_order) > min_ore:
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(self.obs_grid)
self.obs_re_pos = copy.deepcopy(self.obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [
round(self.obs_grid[i][0], 2),
round(self.obs_grid[i][1], 2), self.obs_r[i]
], 2)
self.grid_ori = CUF.obstacle_circle(
self.grid_ori,
[self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [
round(self.obs_re_grid[i][0], 2),
round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]
], 2)
self.grid_del = CUF.obstacle_circle(
self.grid_del,
[self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
obs_info[env.ore_order[i]].r_step = i
can_info = []
for i in range(len(env.can_pos)):
can_info.append((OI('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)
print "# of obstacles", len(obs_info), "\n# of candidates", len(can_info)
for oi in range(len(obs_info)):
for ord in range(len(env.ore_order)):
if obs_info[oi].r_step == ord:
print "\nobstacle ", oi, "will be rearranged in step", ord
CUF.draw_grid_info(env.grid_ori)
CUF.draw_grid_info(env.grid_del)
CUF.draw_grid_info(env.grid_max_can)
plt.show()
'''
GET candidates info
'''
# can_step = []
# for step_i in range(len(ore_grid)): # We removed candidates that occludes the target so '-1' is fine.
# # step1 : get available candidates in this step
# print "\nstep : 1-", step_i, "starts!"
# tmp_can_pan = []
# tmp_can_grid = copy.deepcopy(can_grid)
ws_cen[1] - ws_d * GRID_SIZE * 0.5 - OBJ_RADIUS
]) # right low point
ws_side.append([
ws_cen[0] + ws_w * GRID_SIZE * 0.5 + OBJ_RADIUS,
ws_cen[1] + ws_d * GRID_SIZE * 0.5 + OBJ_RADIUS
]) # right high point
ws_side.append([
ws_cen[0] - ws_w * GRID_SIZE * 0.5,
ws_cen[1] + ws_d * GRID_SIZE * 0.5 + OBJ_RADIUS
]) # left high point
obs_wall = []
print("number of width side wall", ws_w * GRID_SIZE / OBJ_RADIUS)
print("number of depth side wall", ws_d * GRID_SIZE / OBJ_RADIUS)
obs_wall.extend(
CF.linspace2D(ws_side[0], ws_side[1],
round(ws_w * GRID_SIZE / OBJ_RADIUS)))
print("obstacles for wall", len(obs_wall), type(obs_wall), obs_wall)
obs_wall.extend(
CF.linspace2D(ws_side[1], ws_side[2],
round(ws_d * GRID_SIZE / OBJ_RADIUS)))
print("obstacles for wall", len(obs_wall), type(obs_wall), obs_wall)
obs_wall.extend(
CF.linspace2D(ws_side[2], ws_side[3],
round(ws_w * GRID_SIZE / OBJ_RADIUS)))
print("obstacles for wall", len(obs_wall), type(obs_wall), obs_wall)
print("obstacles for wall", len(obs_wall), type(obs_wall), obs_wall)
plt.figure()
for i in range(len(ws_point)):
plt.scatter(ws_side[i][0], ws_side[i][1], color='red', alpha=0.5)
plt.scatter(ws_point[i][0], ws_point[i][1], color='black', alpha=0.8)
# 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)
rearr_ore = len(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)
#
# plt.show()
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))
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)
CUF.draw_grid_info(env.grid_ori)
CUF.draw_grid_info(env.grid_del)
CUF.draw_grid_info(env.grid_max_can)
while 1:
# Check C.A : just next step
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):
in_obs_pos.remove(env.obs_pos[env.ore_order[ore_i]])
t_can_info.append(
env.get_can_A(in_can_info, in_obs_pos, env.tar_pos))
# Check C.BT
ws_side.append([
ws_cen[0] + ws_w * GRID_SIZE * 0.5 + OBJ_R,
ws_cen[1] - ws_d * GRID_SIZE * 0.5 - OBJ_R
]) # right low point
ws_side.append([
ws_cen[0] + ws_w * GRID_SIZE * 0.5 + OBJ_R,
ws_cen[1] + ws_d * GRID_SIZE * 0.5 + OBJ_R
]) # right high point
ws_side.append([
ws_cen[0] - ws_w * GRID_SIZE * 0.5,
ws_cen[1] + ws_d * GRID_SIZE * 0.5 + OBJ_R
]) # left high point
obs_wall = []
obs_wall.extend(
CUF.linspace2D(ws_side[0], ws_side[1],
round(ws_w * GRID_SIZE / OBJ_R)))
obs_wall.extend(
CUF.linspace2D(ws_side[1], ws_side[2],
round(ws_d * GRID_SIZE / OBJ_R)))
obs_wall.extend(
CUF.linspace2D(ws_side[2], ws_side[3],
round(ws_w * GRID_SIZE / OBJ_R)))
ws_zero = [
round(ws_cen[0] - ws_w * GRID_SIZE * 0.5, 2),
round(ws_cen[1] - ws_d * GRID_SIZE * 0.5, 2)
]
print("ws zero point", ws_zero)
# GRID_SIZE = 0.01
grid_acc = np.zeros([ws_w, ws_d])
def get_env_case1(self):
while 1:
obs_r = [0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035]
tar_r = 0.035
min_ore = 2
self.obs_grid = []
grid_tmp = copy.deepcopy(self.grid_act)
self.obs_r = obs_r
self.tar_r = tar_r
self.obs_grid = [[5, 70], [5, 50], [5, 10], [20, 70], [20, 50],
[15, 30], [22, 25], [20, 10]]
self.tar_grid = [30, 30]
# self.grid_ori = copy.deepcopy(self.grid_act)
# for i in range(len(self.obs_r)):
# print "obs", i, round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2)
# self.grid_ori = CUF.obstacle_circle(self.grid_ori, [round(self.obs_grid[i][0], 2), round(self.obs_grid[i][1], 2), self.obs_r[i]], 2)
# self.grid_ori = CUF.obstacle_circle(self.grid_ori, [self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
# break
# for ri in self.obs_r:
# grid_tmp, obs_center_tmp = CUF.place_circle_object_ig(grid_tmp, ri, 2)
# self.obs_grid.append(obs_center_tmp)
# grid_tmp, tar_tmp = CUF.place_circle_object_ig(grid_tmp, self.tar_r, 4)
# self.tar_grid = copy.deepcopy(tar_tmp)
self.obs_pos = []
for i in self.obs_grid:
xi, yi = i
self.obs_pos.append([
round(xi * self.GRID_SIZE + self.ws_zero[0], 2),
round(yi * self.GRID_SIZE + self.ws_zero[1], 2)
])
self.tar_pos = [
round(self.tar_grid[0] * self.GRID_SIZE + self.ws_zero[0], 2),
round(self.tar_grid[1] * self.GRID_SIZE + self.ws_zero[1], 2)
] # target object!
self.d_max = 2.0
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_tar_ori = [0.0, 0.0, 0.0]
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tm_obs_ori = []
for i in range(ob):
tm_obs_ori.append([0.0, 0.0, 0.0])
ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos,
tm_obs_ori, self.rob_pos, self.rob_pos,
self.d_max)
if len(ore_order) > min_ore:
print "environment setting OK"
print "target", self.tar_pos
print "obstacles", self.obs_pos
print "remove", ore_order, "th obstacles"
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
self.ore_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori,
tm_obs_pos, tm_obs_ori,
self.rob_pos, self.rob_pos,
self.d_max)
# check if the obstacles are rearranged then target is reachable
tm_tar_pos = copy.deepcopy(self.tar_pos)
tm_obs_pos = copy.deepcopy(self.obs_pos)
tm_ore_pos = []
for i in self.ore_order:
if i != 'T':
tm_ore_pos.append(self.obs_pos[i])
for i in self.ore_order:
if i != 'T':
tm_obs_pos.remove(self.obs_pos[i])
tm_obs_pos.extend(self.obs_wall)
ob = len(tm_obs_pos)
tmp_order = TM_noplot(ob, tm_tar_pos, tm_tar_ori, tm_obs_pos,
tm_obs_ori, self.rob_pos, self.rob_pos,
self.d_max)
if len(tmp_order) == 1:
break
'''
# with out additional rearrangement
'''
if len(self.ore_order) > min_ore:
self.ore_grid = []
self.ore_pos = []
self.ore_r = []
self.obs_re_grid = copy.deepcopy(self.obs_grid)
self.obs_re_pos = copy.deepcopy(self.obs_pos)
self.obs_re_r = copy.deepcopy(self.obs_r)
for i in self.ore_order:
if i != 'T':
self.ore_grid.append(self.obs_re_grid[i])
self.ore_pos.append(self.obs_re_pos[i])
self.ore_r.append(self.obs_re_r[i])
for i in self.ore_order:
if i != 'T':
self.obs_re_grid.remove(self.obs_grid[i])
self.obs_re_pos.remove(self.obs_pos[i])
self.obs_re_r.remove(self.obs_r[i])
self.grid_ori = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_r)):
self.grid_ori = CUF.obstacle_circle(self.grid_ori, [
round(self.obs_grid[i][0], 2),
round(self.obs_grid[i][1], 2), self.obs_r[i]
], 2)
self.grid_ori = CUF.obstacle_circle(
self.grid_ori,
[self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.grid_del = copy.deepcopy(self.grid_act)
for i in range(len(self.obs_re_r)):
self.grid_del = CUF.obstacle_circle(self.grid_del, [
round(self.obs_re_grid[i][0], 2),
round(self.obs_re_grid[i][1], 2), self.obs_re_r[i]
], 2)
self.grid_del = CUF.obstacle_circle(
self.grid_del,
[self.tar_grid[0], self.tar_grid[1], self.tar_r], 4) # target
self.ore_order.pop()
t_cp_index = []
in_can_info = copy.deepcopy(t_can_add)
ret_can, ret_index = env.get_cp(in_can_info)
t_cp = ret_can
t_cp_index = ret_index
print "\n Our Cp:", t_cp, len(t_cp), "index", t_cp_index
# step2 : check c_ore for each cp and pick min of it
c_ore = []
in_can_info = copy.deepcopy(t_cp)
c_ore = env.get_c_ore(in_can_info)
print "cp", t_cp, "c_ore", c_ore
min_c_ore = CUF.min_len_list(c_ore)
print "c ore:", c_ore, "min c ore:", min_c_ore
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)])
cp = copy.deepcopy(t_cp[c_ore.index(min_c_ore)])
print "cp", cp
# 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_can_info.remove(in_can_info[t_cp_index[t_cp.index(t_cp[c_ore.index(min_c_ore)])]])
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, env.tar_pos))
#
# 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]])
extra_try_cnt = 0
while len(s_e):
# print "extra try loop:", extra_try_cnt #: to find how many loop we use
extra_try_cnt = extra_try_cnt + 1
# 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
# if rearr_cnt > 10:
# CUF.draw_environment(env.ws_zero, [env.ws_w/100.0, env.ws_d/100.0], env.rob_pos, env.obs_pos, env.can_pos, env.tar_pos)
# # CUF.draw_check_pos(cp.pos, 'purple')
# plt.show()
## step3 : "get t_cf", check candidates which have A = 1 and BT' = 0
## Check A for this environment state T' is t_cp_i
def compute_empty_slots(ws_size, ws_center_pos, tar_pos, target_r, objs_pos, objs_r):
import copy
import numpy as np
import S_custom_function as CUF
from VFHplus_change_radius import influence
from D_0909_envClass4altest import EnvInfo as EI
from D_0909_envClass4altest import CanInfo as CI
GRID_SIZE = 0.01
ws_width = int(ws_size[0]*100)
ws_depth = int(ws_size[1]*100)
grid_init = np.zeros([ws_width, ws_depth])
grid_act = CUF.mark_edge_grid(grid_init)
print "\ngrid size:", np.shape(grid_act)
for obj_i in range(len(objs_pos)):
obj_i_grid = [round(ws_width/2 - (ws_center_pos[0] - objs_pos[obj_i][0])*100, 1), round(ws_depth/2 - (ws_center_pos[1] - objs_pos[obj_i][1])*100, 1)]
print "obj pos:", objs_pos[obj_i]
print "obj grid:", round(obj_i_grid[0]), round(obj_i_grid[1])
obstacle_xyr_grid = [obj_i_grid[0], obj_i_grid[1], objs_r[obj_i]]
grid_act = CUF.obstacle_circle(grid_act, obstacle_xyr_grid, 2)
# Considering the y axis of the target while packing circles
tar_grid = [round(ws_width/2 - (ws_center_pos[0] - tar_pos[0])*100, 1), round(ws_depth/2 - (ws_center_pos[1] - tar_pos[1])*100, 1)]
print "\nTarget pos:", tar_pos
print "Target grid:", tar_grid
print "consider => target y - r*1.5 ~ target.y + r * 1.5"
print "target r * 3 => grid length:", int(target_r*100*3)
for x_i in range(ws_width):
for y_i in range(int(target_r*100*1.5)):
if int(tar_grid[1])+y_i < ws_depth and int(tar_grid[1])+y_i > 0:
#print tar_grid[1], y_i
grid_act[x_i][int(tar_grid[1])+y_i] = 2
grid_act[x_i][int(tar_grid[1])-y_i] = 2
else:
print "out of plane"
grid_ori = grid_act
bt_num = 3
trial_num = 2000
bt_circle = []
circle_r = target_r
for bt in range(bt_num):
can_grid = []
grid_can = copy.deepcopy(grid_ori) # get original scene from the grid_set
empt_grid, occu_grid = CUF.getEmpOcc(grid_can)
for i in range(trial_num):
pick_cen = np.random.randint(0, len(empt_grid))
check_sum = 0
for oc in range(len(occu_grid)):
d_w = empt_grid[pick_cen][0] - occu_grid[oc][0]
d_d = empt_grid[pick_cen][1] - occu_grid[oc][1]
d_c = (d_w * d_w + d_d * d_d) ** 0.5 * GRID_SIZE
if d_c <= circle_r:
check_sum = 1
if check_sum == 0:
can_grid.append(empt_grid[pick_cen])
for em in range(len(empt_grid)):
d_w = empt_grid[pick_cen][0] - empt_grid[em][0]
d_d = empt_grid[pick_cen][1] - empt_grid[em][1]
d_c = (d_w * d_w + d_d * d_d) ** 0.5 * GRID_SIZE
if d_c <= circle_r:
grid_can[empt_grid[em][0]][empt_grid[em][1]] = 3
grid_can[empt_grid[pick_cen][0]][empt_grid[pick_cen][1]] = 3
occu_grid.append([empt_grid[em][0], empt_grid[em][1]])
bt_circle.append([can_grid, grid_can])
max_cir_num = []
for i in range(len(bt_circle)):
max_cir_num.append([len(bt_circle[i][0])])
print(max_cir_num.index(max(max_cir_num)))
max_trial = max_cir_num.index(max(max_cir_num))
grid_max_can = copy.deepcopy(bt_circle[max_trial][1])
t_can_grid = bt_circle[max_trial][0]
can_pos = []
#ws_zero = [round(ws_cen[0] - ws_width * GRID_SIZE * 0.5, 2), round(ws_cen[1] - ws_depth * GRID_SIZE * 0.5, 2)]
ws_zero = [ws_center_pos[0] - ws_size[0] * 0.5, ws_center_pos[1] - ws_size[1] * 0.5]
for i in t_can_grid:
xi, yi = i
can_pos.append([ws_zero[0] + xi * GRID_SIZE, ws_zero[1] + yi * GRID_SIZE])
return can_pos, grid_max_can
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_d, ws_w = int(round(ws[2][1] * 100)), int(round(ws[2][0] * 100))
print "work space width, depth", ws_w, ws_d
# GRID_SIZE = 0.01
ws_zero_pos = [
round(ws[0][2] - ws[2][0] * 0.5, 2),
round(-ws[0][1] - ws[2][1] * 0.5, 2)
]
print "ws, zero pos", ws_zero_pos
grid_acc = np.zeros([ws_w, ws_d])
grid_acc = CUF.mark_edge_grid(grid_acc)
rob_pos = [0.0, 0.0]
obs_r = []
obs_pos = []
obs_ori = []
obs_grid = []
tar_pos = [round(target_info[0][0][2], 2), round(-target_info[0][0][1], 2)]
tar_grid = [
int(round((target_info[0][0][2] - ws_zero_pos[0]) * 100)),
int(round((-target_info[0][0][1] - ws_zero_pos[1]) * 100))
]
tar_r = [round(target_info[0][2][1] * 0.5, 2)]
print "number of obstacles", len(obstacle_info)
for i in range(len(obstacle_info)):
obs_pos.append([
round(obstacle_info[i][0][2], 2),
# for i in range(len(t_cp)):
# print "cp", t_cp[i].pos, "\nc_ore", c_ore[i]
while 1:
for i in range(len(c_ore)):
if c_ore[i] == []:
c_ore[i] = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
for i in range(len(c_ore)):
for j in range(len(c_ore[i])):
if c_ore[i][j] == len(
env.obs_pos
) + 1: # this is when env.tar is in the list
c_ore[i] = [
1, 2, 3, 4, 5, 6, 7, 8, 9, 10
]
min_c_ore = CUF.min_len_list(c_ore)
# print "min c ore:", min_c_ore
#
# 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(t_cp[c_ore.index(min_c_ore)])
# print "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,
# 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]])
extra_try_cnt = 0
while len(s_e):
print "extra try loop:", extra_try_cnt #: to find how many loop we use
extra_try_cnt = extra_try_cnt + 1
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
if rearr_cnt > 10:
CUF.draw_environment(
env.ws_zero,
[env.ws_w / 100.0, env.ws_d / 100.0],
env.rob_pos, env.obs_pos, env.can_pos,
env.tar_pos)
# CUF.draw_check_pos(cp.pos, 'purple')
plt.show()
env.obs_grid = copy.deepcopy(ori_obs_grid)
env.can_pos = copy.deepcopy(ori_can_pos)
env.can_grid = copy.deepcopy(ori_can_grid)
env.update_env(env.obs_pos, env.obs_grid)
can_info = [] #: candidate information list!
for i in range(len(env.can_pos)):
can_info.append(CI('candidate', env.can_pos[i], env.can_grid[i]))
t_ore_order = copy.deepcopy(env.ore_order)
rearr_ore = len(env.ore_order)
# CUF.draw_environment(env.ws_zero, [env.ws_w/100.0, env.ws_d/100.0], env.rob_pos, env.obs_pos, env.can_pos, env.tar_pos)
# CUF.draw_check_pos([0.5, 0.5])
# plt.show()
end_t_cp_flag = 0
CUF.draw_environment(env.ws_zero, [env.ws_w / 100.0, env.ws_d / 100.0], env.rob_pos, env.obs_pos,
env.can_pos, env.tar_pos)
print "\n Test", test_i, "used method", method, "order(len):", env.ore_order, "(", len(env.ore_order), ")", "# can:", len(can_info), "# obs:", len(obs_r)
while len(env.ore_order): # this while loop is for the algorithm algorithm will go on until it can access to target
print "\niter:", rearr_cnt
print "OR:", env.ore_order
if rearr_cnt > 20:
print "acc check error"
break
# Check C.A : just next step
t_can_info = [] #: temp value of candidate information
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]])
t_can_info = env.get_can_A(in_can_info, in_obs_pos, env.tar_pos)