def randomizedPrim(dict) :
"""
Creates a random maze.
Args:
dict: Dictionary with all the configuration settings.
Returns:
walls: Walls placed by the algorithm.
source: Position of a free cell to place source.
destination: Position of a free cell to place destination.
"""
# Intialize environment
env = Environment(dict, [])
# Initialize the grid as all walls and find a start cell
for row in env.grid:
for cell in row:
if cell.type != 'source' and cell.type != 'destination':
cell.type = 'wall'
src = cell
cell.type = 'free'
blocked = getBlockedCells(src, env)
while len(blocked) :
cell = blocked.pop()
free = getFreeCells(cell, env)
newFree = mid(free, cell, env)
if newFree.type == 'wall':
newFree.type = 'free'
if cell.type == 'wall':
cell.type = 'free'
S1 = getBlockedCells(cell, env)
for i in S1:
blocked.add(i)
# Append grid changes
gridChanges = []
flag = 1
for row in env.grid:
count = 0
for cell in row:
count += 1
if cell.type == 'wall':
gridChange = {'x': cell.location.x,
'y': cell.location.y}
gridChanges.append(gridChange)
else:
dst = {'x': cell.location.x,
'y': cell.location.y}
if flag and count > 2:
flag = 0
src = {'x': cell.location.x,
'y': cell.location.y}
return {'walls':gridChanges, 'source':src, 'destination':dst}
def train_process(cfg: Config, render_buffer: mp.Queue):
"""
Main train loop
"""
# Global environment
cfg.global_deterministic()
device = torch.device(cfg.device)
Profiler.enable_profiling(cfg.profiling)
# to_print = []
# for num_agents in [128]:
# cfg.total_agents = num_agents
Profiler.reset()
# Init
env = Environment(cfg, device)
ctrl = Controller(cfg, env, device)
# stats = []
# Run training or inference
for i_episode in range(1, cfg.total_episode + 1):
with Profiler('episode'):
stat = ctrl.run_episode(i_episode, render_buffer)
# stats.append(stat)
# Finishing
# mean_stat = Stat(*tuple(sum(stat) / len(stat)for stat in zip(*stats)))
t_step, t_eps = Profiler.print_all(ctrl.total_step, cfg.total_episode)
# to_print.append((num_agents, mean_stat, t_step, t_eps))
# for log in to_print:
# print('Agents %d. Success %6.2f%%. Reward %5.2f. Collide %5.2f. Step %4.0f. Timeout %4.1f. Entropy %4.2f' % (
# log[0], *log[1]))
# print('Step %d. Episode %d' % (log[2], log[3]))
render_buffer.put(None)
def pile_scenario(n, vis, output, debug):
objects = YcbObjects('objects/ycb_objects',
mod_orn=['ChipsCan', 'MustardBottle',
'TomatoSoupCan'],
mod_stiffness=['Strawberry'],
exclude=['CrackerBox', 'Hammer'])
center_x, center_y = 0.05, -0.52
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
camera = Camera((center_x, center_y, 1.9), (center_x,
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
env = Environment(camera, vis=vis, debug=debug, finger_length=0.06)
generator = GraspGenerator(network_path, camera, 5)
for i in range(n):
print(f'Trial {i}')
straight_fails = 0
objects.shuffle_objects()
env.move_away_arm()
info = objects.get_n_first_obj_info(5)
env.create_pile(info)
straight_fails = 0
while len(env.obj_ids) != 0 and straight_fails < 3:
env.move_away_arm()
env.reset_all_obj()
rgb, depth, _ = camera.get_cam_img()
grasps, save_name = generator.predict_grasp(
rgb, depth, n_grasps=3, show_output=output)
for i, grasp in enumerate(grasps):
x, y, z, roll, opening_len, obj_height = grasp
if vis:
debugID = p.addUserDebugLine(
[x, y, z], [x, y, 1.2], [0, 0, 1], lineWidth=3)
succes_grasp, succes_target = env.grasp(
(x, y, z), roll, opening_len, obj_height)
if vis:
p.removeUserDebugItem(debugID)
if succes_target:
straight_fails = 0
if save_name is not None:
os.rename(save_name + '.png', save_name +
f'_SUCCESS_grasp{i}.png')
break
else:
straight_fails += 1
if straight_fails == 3 or len(env.obj_ids) == 0:
break
env.reset_all_obj()
env.remove_all_obj()
def isolated_obj_scenario(n, vis, output, debug):
objects = YcbObjects('objects/ycb_objects',
mod_orn=['ChipsCan', 'MustardBottle',
'TomatoSoupCan'],
mod_stiffness=['Strawberry'])
center_x, center_y = 0.05, -0.52
network_path = 'network/trained-models/cornell-randsplit-rgbd-grconvnet3-drop1-ch32/epoch_19_iou_0.98'
camera = Camera((center_x, center_y, 1.9), (center_x,
center_y, 0.785), 0.2, 2.0, (224, 224), 40)
env = Environment(camera, vis=vis, debug=debug)
generator = GraspGenerator(network_path, camera, 5)
objects.shuffle_objects()
for _ in range(n):
for obj_name in objects.obj_names:
print(obj_name)
path, mod_orn, mod_stiffness = objects.get_obj_info(obj_name)
env.load_isolated_obj(path, mod_orn, mod_stiffness)
env.move_away_arm()
rgb, depth, _ = camera.get_cam_img()
grasps, save_name = generator.predict_grasp(
rgb, depth, n_grasps=3, show_output=output)
for i, grasp in enumerate(grasps):
x, y, z, roll, opening_len, obj_height = grasp
if vis:
debug_id = p.addUserDebugLine(
[x, y, z], [x, y, 1.2], [0, 0, 1], lineWidth=3)
succes_grasp, succes_target = env.grasp(
(x, y, z), roll, opening_len, obj_height)
if vis:
p.removeUserDebugItem(debug_id)
if succes_target:
if save_name is not None:
os.rename(save_name + '.png', save_name +
f'_SUCCESS_grasp{i}.png')
break
env.reset_all_obj()
env.remove_all_obj()
def recursiveMaze(dict):
"""
Creates a random maze.
Args:
dict: Dictionary with all the configuration settings.
Returns:
walls: Walls placed by the algorithm.
source: Position of a free cell to place source.
destination: Position of a free cell to place destination.
"""
# Intialize the environment
env = Environment(dict, [])
def generate(left, right, top, bottom):
if left >= right or top >= bottom:
return
if left >= right - 1 and top >= bottom - 1:
return
rnd = random.randrange(0, 2)
if left >= right - 1:
rnd = 0
if top >= bottom - 1:
rnd = 1
# Horizontally divide the grid
if(rnd == 0):
row = randomEvenNumber(top, bottom)
for i in range(left, right + 1):
if env.grid[row][i].type != 'source' and env.grid[row][i].type != 'destination':
env.grid[row][i].type = 'wall'
i = randomOddNumber(left, right)
if env.grid[row][i].type != 'source' and env.grid[row][i].type != 'destination':
env.grid[row][i].type = 'free'
generate(left, right, top, row - 1)
generate(left, right, row + 1, bottom)
# Vertically divide the grid
else:
clm = randomEvenNumber(left, right)
for i in range(top, bottom + 1):
if env.grid[i][clm].type != 'source' and env.grid[i][clm].type != 'destination':
env.grid[i][clm].type = 'wall'
i = randomOddNumber(top, bottom)
if env.grid[i][clm].type != 'source' and env.grid[i][clm].type != 'destination':
env.grid[i][clm].type = 'free'
generate(left, clm - 1, top, bottom)
generate(clm + 1, right, top, bottom)
generate(0, env.breadth - 1, 0, env.length - 1)
gridChanges = []
flag = 1
# Append grid changes
for row in env.grid:
count = 0
for cell in row:
count += 1
if cell.type == 'wall':
gridChange = {'x': cell.location.x,
'y': cell.location.y}
gridChanges.append(gridChange)
else:
dst = {'x': cell.location.x,
'y': cell.location.y}
if flag and count > 2:
flag = 0
src = {'x': cell.location.x,
'y': cell.location.y}
return {'walls':gridChanges, 'source':src, 'destination':dst}