def main():
parameters = parse_parameters()
save_dir = make_and_get_save_dir(parameters)
file_path = save_dir + '/seed_' + str(parameters.planner_seed) + '_pidx_' + str(parameters.pidx) + '.pkl'
quit_if_already_tested(file_path, parameters.f)
# for creating problem
np.random.seed(parameters.pidx)
random.seed(parameters.pidx)
is_two_arm_env = parameters.domain.find('two_arm') != -1
if is_two_arm_env:
environment = Mover(parameters.pidx)
else:
environment = OneArmMover(parameters.pidx)
environment.initial_robot_base_pose = get_body_xytheta(environment.robot)
if parameters.domain == 'two_arm_mover':
goal_region = 'home_region'
if parameters.n_objs_pack == 4:
goal_object_names = ['square_packing_box1', 'square_packing_box2', 'rectangular_packing_box3',
'rectangular_packing_box4']
else:
goal_object_names = ['square_packing_box1']
environment.set_goal(goal_object_names, goal_region)
elif parameters.domain == 'one_arm_mover':
goal_region = 'rectangular_packing_box1_region'
assert parameters.n_objs_pack == 1
goal_object_names = ['c_obst1']
environment.set_goal(goal_object_names, goal_region)
goal_entities = goal_object_names + [goal_region]
# for randomized algorithms
np.random.seed(parameters.planner_seed)
random.seed(parameters.planner_seed)
if parameters.v:
utils.viewer()
environment.set_motion_planner(BaseMotionPlanner(environment, 'rrt'))
# from manipulation.bodies.bodies import set_color
# set_color(environment.env.GetKinBody(goal_object_names[0]), [1, 0, 0])
start_time = time.time()
n_mp = n_ik = 0
goal_object_names, high_level_plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names,
start_time,
parameters) # finds the plan
total_time_taken = time.time() - start_time
n_mp += mp
n_ik += ik
total_n_nodes = 0
total_plan = []
idx = 0
found_solution = False
timelimit = parameters.timelimit
while total_time_taken < timelimit:
goal_obj_name = goal_object_names[idx]
plan, n_nodes, status, (mp, ik) = find_plan_for_obj(goal_obj_name, high_level_plan[idx], environment,
start_time,
timelimit, parameters)
total_n_nodes += n_nodes
total_time_taken = time.time() - start_time
print goal_obj_name, goal_object_names, total_n_nodes
print "Time taken: %.2f" % total_time_taken
if total_time_taken > timelimit:
break
if status == 'HasSolution':
execute_plan(plan)
environment.initial_robot_base_pose = utils.get_body_xytheta(environment.robot)
total_plan += plan
save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities,
total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters)
idx += 1
else:
# Note that HPN does not have any recourse if this happens. We re-plan at the higher level.
goal_object_names, plan, (mp, ik) = find_plan_without_reachability(environment, goal_object_names,
start_time, parameters) # finds the plan
n_mp += mp
n_ik += ik
total_plan = []
idx = 0
if idx == len(goal_object_names):
found_solution = True
break
else:
idx %= len(goal_object_names)
total_time_taken = time.time() - start_time
save_plan(total_plan, total_n_nodes, len(goal_object_names) - idx, found_solution, file_path, goal_entities,
total_time_taken, {'mp': n_mp, 'ik': n_ik}, parameters)
print 'plan saved'
def main():
parameters = parse_parameters()
save_dir = make_and_get_save_dir(parameters)
file_path = save_dir + '/seed_' + str(
parameters.planner_seed) + '_pidx_' + str(parameters.pidx) + '.pkl'
quit_if_already_tested(file_path, parameters.f)
# for creating problem
np.random.seed(parameters.pidx)
random.seed(parameters.pidx)
is_one_arm_env = parameters.domain.find('two_arm') != -1
if is_one_arm_env:
environment = Mover(parameters.pidx)
goal_region = ['home_region']
else:
environment = OneArmMover(parameters.pidx)
goal_region = ['rectangular_packing_box1_region']
goal_object_names = [
obj.GetName() for obj in environment.objects[:parameters.n_objs_pack]
]
goal_entities = goal_object_names + goal_region
# for randomized algorithms
np.random.seed(parameters.planner_seed)
random.seed(parameters.planner_seed)
if parameters.v:
environment.env.SetViewer('qtcoin')
# from manipulation.bodies.bodies import set_color
# set_color(environment.env.GetKinBody(goal_object_names[0]), [1, 0, 0])
stime = time.time()
goal_object_names, high_level_plan = find_plan_without_reachability(
environment, goal_object_names) # finds the plan
total_n_nodes = 0
total_plan = []
idx = 0
total_time_taken = 0
found_solution = False
timelimit = parameters.timelimit
timelimit = np.inf
while total_n_nodes < 1000 and total_time_taken < timelimit:
goal_obj_name = goal_object_names[idx]
plan, n_nodes, status = find_plan_for_obj(goal_obj_name,
high_level_plan[idx],
environment, stime,
timelimit)
total_n_nodes += n_nodes
total_time_taken = time.time() - stime
print goal_obj_name, goal_object_names, total_n_nodes
print "Time taken: %.2f" % total_time_taken
if status == 'HasSolution':
execute_plan(plan)
environment.initial_robot_base_pose = utils.get_body_xytheta(
environment.robot)
total_plan += plan
save_plan(total_plan, total_n_nodes,
len(goal_object_names) - idx, found_solution, file_path,
goal_entities, total_time_taken)
idx += 1
else:
# Note that HPN does not have any recourse if this happens. We re-plan at the higher level.
goal_object_names, plan = find_plan_without_reachability(
environment, goal_object_names) # finds the plan
total_plan = []
idx = 0
if idx == len(goal_object_names):
found_solution = True
break
else:
idx %= len(goal_object_names)
save_plan(total_plan, total_n_nodes,
len(goal_object_names) - idx, found_solution, file_path,
goal_entities, total_time_taken)
print 'plan saved'