def obs_gen(N, N_obs, N_pc=1400, width=4):
MIN_X = -25
MAX_X = 25
MIN_Y = -35
MAX_Y = 35
obs_list = []
near = width * 2
print('generating obs...')
for i in range(N):
obs_single = []
for j in range(N_obs):
low_x = -25 + width / 2
high_x = 25 - width / 2
low_y = -35 + width / 2
high_y = 35 - width / 2
while True:
# randomly sample in the entire space
obs = np.random.uniform(low=[low_x, low_y],
high=[high_x, high_y])
# check if it is near enough to previous obstacles
too_near = False
for k in range(len(obs_single)):
if np.linalg.norm(obs - obs_single[k]) < near:
too_near = True
break
if not too_near:
break
obs_single.append(obs)
obs_single = np.array(obs_single)
obs_list.append(obs_single)
obs_list = np.array(obs_list)
# convert from obs to point cloud
obc_list = rectangle_pcd(obs_list, width, N_pc)
return obs_list, obc_list
def obs_gen(N, N_obs, N_pc=1400, width=4):
# generate one obs in each phase
LENGTH = 20.
near = LENGTH * 1.2
obs_list = []
bottom_threshold = width / 10
for i in range(N):
obs_single = []
for j in range(N_obs):
'''
The obstacle should be inside the maximal circle, but also not blocking (0,0)
'''
while True:
obs = np.random.normal(size=2)
obs = obs / np.linalg.norm(obs) * (LENGTH * 2 - LENGTH / 2)
if j % 4 == 0:
obs = np.abs(obs) # +,+
elif j % 4 == 1:
obs = np.abs(obs)
obs[0] = -obs[0] # -,+
elif j % 4 == 2:
obs = np.abs(obs)
obs[1] = -obs[1] # +,-
elif j % 4 == 3:
obs = -np.abs(obs)
while True:
# make sure it does not block (0,0)
alpha = np.random.uniform(low=0.8, high=1.)
obs_ = alpha * obs
# make sure that when the obstacle is below x-axis, it does not intersect with the pole
if j % 4 == 1:
obs_[0] = min(obs_[0], -width / 2 - bottom_threshold)
if j % 4 == 2:
obs_[0] = max(obs_[0], width / 2 + bottom_threshold)
if np.abs(obs_).max() > width / 2:
obs = obs_
break
# see if it is cluttered enough by making sure obstacles can't be
# too close
too_near = False
for k in range(len(obs_single)):
if np.linalg.norm(obs - obs_single[k]) < near:
too_near = True
#print("too near")
break
if not too_near:
#print('not too near')
break
obs_single.append(obs)
obs_single = np.array(obs_single)
obs_list.append(obs_single)
obs_list = np.array(obs_list)
# convert from obs to point cloud
obc_list = rectangle_pcd(obs_list, width, N_pc)
return obs_list, obc_list
def obs_gen(N, N_obs, N_pc=1400, width=4):
H = 0.5
L = 2.5
low_h = -width / 2 - L
high_h = width / 2 + L
# predefined y values
y_up = H + width / 2 + L / 2
y_down = H - width / 2 - L / 2
near = width * 1.2
obs_list = []
for i in range(N):
obs_single = []
for j in range(N_obs):
'''
make sure the obstacle does not block the pole entirely
by making sure the fixed point of the pole is not in the obs
hence the valid range for y axis is:
H + low_h ~ - width/2, H + width/2 ~ H + high_h
'''
while True:
# first randomly see if it is left or right
side = np.random.randint(low=0, high=2)
# 0: left, 1: right
if side == 0:
#obs = np.random.uniform(low=[-15, H+low_h], high=[15, -width/2])
obs = np.random.uniform(low=[-17, y_down],
high=[17, y_down])
else:
#obs = np.random.uniform(low=[-15, H+width/2], high=[15, H+high_h])
obs = np.random.uniform(low=[-17, y_up], high=[17, y_up])
too_near = False
for k in range(len(obs_single)):
if np.linalg.norm(obs - obs_single[k]) < near:
too_near = True
break
if not too_near:
break
obs_single.append(obs)
obs_single = np.array(obs_single)
obs_list.append(obs_single)
obs_list = np.array(obs_list)
# convert from obs to point cloud
obc_list = rectangle_pcd(obs_list, width, N_pc)
return obs_list, obc_list
obs = np.random.uniform(low=[low_x, low_y], high=[high_x, high_y])
# check if it is near enough to previous obstacles
too_near = False
for k in range(len(obs_single)):
if np.linalg.norm(obs - obs_single[k]) < near:
too_near = True
break
if not too_near:
break
obs_single.append(obs)
obs_single = np.array(obs_single)
obs_list.append(obs_single)
obs_list = np.array(obs_list)
# convert from obs to point cloud
obc_list = rectangle_pcd(obs_list, width, 1400)
print('generated.')
print(obs_list.shape)
# Create custom system
#obs_list = [[-10., -3.],
# [0., 3.],
# [10, -3.]]
obs_list = obs_list[0]
obs_list_to_corner = []
for i in range(len(obs_list)):
obs_list_to_corner_i = []
obs_list_to_corner_i.append(obs_list[i][0] - width / 2)
obs_list_to_corner_i.append(obs_list[i][1] - width / 2)
obs_list_to_corner_i.append(obs_list[i][0] + width / 2)