def find_plan_for_obj(obj_name, target_op_inst, environment, stime, timelimit, parameters):
is_one_arm_environment = environment.name.find('one_arm') != -1
if is_one_arm_environment:
target_op_inst = attach_q_goal_as_low_level_motion(target_op_inst)
swept_volumes = PickAndPlaceSweptVolume(environment, None)
swept_volumes.add_pap_swept_volume(target_op_inst)
obstacles_to_remove = swept_volumes.get_objects_in_collision()
print len(obstacles_to_remove)
if len(obstacles_to_remove) == 0:
return [target_op_inst], 1, "HasSolution", (0, 0)
rsc = OneArmResolveSpatialConstraints(problem_env=environment,
goal_object_name=obj_name,
goal_region_name='rectangular_packing_box1_region',
config=parameters)
obstacle_to_remove_idx = 0
else:
rsc = ResolveSpatialConstraints(problem_env=environment,
goal_object_name=obj_name,
goal_region_name='home_region',
misc_region_name='loading_region')
plan = None
plan_found = False
status = 'NoSolution'
while not plan_found and rsc.get_num_nodes() < 100 and time.time() - stime < timelimit:
if is_one_arm_environment:
obj_to_move = obstacles_to_remove[obstacle_to_remove_idx]
tmp_obstacles_to_remove = set(obstacles_to_remove).difference(set([obj_to_move]))
tmp_obstacles_to_remove = list(tmp_obstacles_to_remove)
top_level_plan = [target_op_inst]
plan, status = rsc.search(object_to_move=obj_to_move,
parent_swept_volumes=swept_volumes,
obstacles_to_remove=tmp_obstacles_to_remove,
objects_moved_before=[target_op_inst.discrete_parameters['object']],
plan=top_level_plan,
stime=stime,
timelimit=timelimit)
else:
plan, status = rsc.search(obj_name,
parent_swept_volumes=None,
obstacles_to_remove=[],
objects_moved_before=[],
plan=[],
ultimate_plan_stime=stime,
timelimit=timelimit)
plan_found = status == 'HasSolution'
if plan_found:
print "Solution found"
else:
if is_one_arm_environment:
obstacle_to_remove_idx += 1
if obstacle_to_remove_idx == len(obstacles_to_remove):
break
print "Restarting..."
if plan_found:
return plan, rsc.get_num_nodes(), status, (rsc.n_mp, rsc.n_ik)
else:
return [], rsc.get_num_nodes(), status, (rsc.n_mp, rsc.n_ik)
class BlocksKeyConfigs(UnaryPredicate):
def __init__(self, problem_env, target_pap):
UnaryPredicate.__init__(self, problem_env)
self.target_pap = target_pap
self.swept_volume = PickAndPlaceSweptVolume(self.problem_env)
self.swept_volume.add_swept_volume(target_pap[0], target_pap[1])
def __call__(self, entity, goal):
is_obj = entity not in self.problem_env.regions
if not is_obj:
return False
entity = self.problem_env.env.GetKinBody(entity)
is_col = self.swept_volume.is_obj_in_collision_with_given_place_volume(
self.target_pap[1].low_level_motion, entity, self.target_pap[0])
return is_col
def search(self, object_to_move, parent_swept_volumes, obstacles_to_remove, objects_moved_before, plan,
stime=None, timelimit=None):
print objects_moved_before
print time.time() - stime, timelimit
if time.time() - stime > timelimit:
return False, 'NoSolution'
utils.set_color(plan[-1].discrete_parameters['object'], [0, 0, 1])
for o in self.problem_env.objects:
if o in objects_moved_before:
utils.set_color(o, [0, 0, 0])
elif o.GetName() in objects_moved_before:
utils.set_color(o, [0, 0, 0])
else:
utils.set_color(o, [0, 1, 0])
set_color(object_to_move, [1, 0, 0])
utils.set_color(plan[-1].discrete_parameters['object'], [0, 0, 1])
# Initialize data necessary for this recursion level
swept_volumes = PickAndPlaceSweptVolume(self.problem_env, parent_swept_volumes)
objects_moved_before = [o for o in objects_moved_before]
plan = [p for p in plan]
self.number_of_nodes += 1
if isinstance(object_to_move, unicode):
object_to_move = self.problem_env.env.GetKinBody(object_to_move)
# Select the region to move the object to
if object_to_move == self.goal_object:
target_region = self.goal_region
else:
obj_curr_region = self.problem_env.get_region_containing(object_to_move)
not_in_box = obj_curr_region.name.find('box') == -1
if not_in_box:
# randomly choose one of the shelf regions
target_region = self.problem_env.shelf_regions.values()[0]
else:
target_region = obj_curr_region
# Get PaP
self.problem_env.set_exception_objs_when_disabling_objects_in_region(objects_moved_before)
pap, status = self.find_pick_and_place(object_to_move, target_region, swept_volumes)
if status != 'HasSolution':
print "Failed to sample pap, giving up on branch"
return False, "NoSolution"
pap = attach_q_goal_as_low_level_motion(pap)
swept_volumes.add_pap_swept_volume(pap)
self.problem_env.enable_objects_in_region('entire_region')
objects_in_collision_for_pap = swept_volumes.get_objects_in_collision_with_last_pap()
# O_{PAST}
prev = obstacles_to_remove
obstacles_to_remove = objects_in_collision_for_pap + [o for o in obstacles_to_remove
if o not in objects_in_collision_for_pap]
# O_{FUC} update
objects_moved_before.append(object_to_move)
plan.insert(0, pap)
if len(obstacles_to_remove) == 0:
return plan, 'HasSolution'
# enumerate through all object orderings
print "Obstacles to remove", obstacles_to_remove
self.problem_env.set_exception_objs_when_disabling_objects_in_region(objects_moved_before)
for new_obj_to_move in obstacles_to_remove:
tmp_obstacles_to_remove = set(obstacles_to_remove).difference(set([new_obj_to_move]))
tmp_obstacles_to_remove = list(tmp_obstacles_to_remove)
print "tmp obstacles to remove:", tmp_obstacles_to_remove
print "Recursing on", new_obj_to_move
branch_plan, status = self.search(new_obj_to_move,
swept_volumes,
tmp_obstacles_to_remove,
objects_moved_before,
plan, stime=stime, timelimit=timelimit)
is_branch_success = status == 'HasSolution'
if is_branch_success:
return branch_plan, status
else:
print "Failed on ", new_obj_to_move
# It should never come down here, as long as the number of nodes have not exceeded the limit
# but to which level do I back track? To the root node. If this is a root node and
# the number of nodes have not reached the maximum, keep searching.
return False, 'NoSolution'
def search(self,
object_to_move,
parent_swept_volumes,
obstacles_to_remove,
objects_moved_before,
plan,
parent_pick=None,
parent_obj=None,
ultimate_plan_stime=None,
timelimit=None):
print time.time() - ultimate_plan_stime
if time.time() - ultimate_plan_stime > timelimit:
return False, 'NoSolution'
swept_volumes = PickAndPlaceSweptVolume(self.problem_env,
parent_swept_volumes)
objects_moved_before = [o for o in objects_moved_before]
plan = [p for p in plan]
self.problem_env.set_exception_objs_when_disabling_objects_in_region(
objects_moved_before)
self.number_of_nodes += 1
object_to_move = self.problem_env.env.GetKinBody(
object_to_move) if isinstance(object_to_move,
unicode) else object_to_move
target_region = self.get_target_region_for_obj(object_to_move)
# Debugging purpose
color_before = get_color(object_to_move)
set_color(object_to_move, [1, 0, 0])
# End of debugging purpose
# PlanGrasp
saver = utils.CustomStateSaver(self.problem_env.env)
stime = time.time()
# this contains mc-path from initial config to the target obj
_, _, pick_operator_instance_for_curr_object = self.get_pick_from_initial_config(
object_to_move)
#print 'Time pick', time.time() - stime
if pick_operator_instance_for_curr_object is None:
saver.Restore()
self.reset()
#print "Infeasible branch"
return False, 'NoSolution'
utils.two_arm_pick_object(
object_to_move,
pick_operator_instance_for_curr_object.continuous_parameters)
# FindPlacements
_, _, place_operator_instance = self.plan_place(
target_region, swept_volumes)
if place_operator_instance is None:
saver.Restore()
self.reset()
print "Infeasible branch"
return False, 'NoSolution'
# O_{FUC} update
objects_moved_before.append(object_to_move)
self.problem_env.set_exception_objs_when_disabling_objects_in_region(
objects_moved_before)
if parent_pick is not None:
utils.two_arm_place_object(
place_operator_instance.continuous_parameters)
_, _, pick_operator_instance_for_parent_object = self.plan_pick_motion_for(
parent_obj, parent_pick) # PlanNavigation
if pick_operator_instance_for_parent_object is None:
print "Infeasible branch"
saver.Restore()
return False, 'NoSolution'
swept_volumes.add_pick_swept_volume(
pick_operator_instance_for_parent_object)
swept_volumes.add_place_swept_volume(
place_operator_instance,
pick_operator_instance_for_curr_object)
plan.insert(0, pick_operator_instance_for_parent_object)
plan.insert(0, place_operator_instance)
else:
pick_operator_instance_for_parent_object = None
swept_volumes.add_place_swept_volume(
place_operator_instance,
pick_operator_instance_for_curr_object)
plan.insert(0, place_operator_instance)
saver.Restore()
# O_{PAST}
self.problem_env.enable_objects_in_region('entire_region')
objs_in_collision_for_pap \
= swept_volumes.get_objects_in_collision_with_pick_and_place(pick_operator_instance_for_parent_object,
place_operator_instance)
obstacles_to_remove = objs_in_collision_for_pap + obstacles_to_remove
# Note:
# For this code to be precisely HPN, I need to keep all objects that have not been moved so far in obstacles
# to remove. I am making the assumption that, because we are in a continuous domain, we always keep the
# tried-actions in the queue, and because the swept-volume heuristic tells us to move the ones in collision
# first, we will always try to move the first-colliding object.
if len(obstacles_to_remove) == 0:
objs_in_collision_for_picking_curr_obj \
= swept_volumes.pick_swept_volume.get_objects_in_collision_with_given_op_inst(
pick_operator_instance_for_curr_object)
if len(objs_in_collision_for_picking_curr_obj) == 0:
plan.insert(0, pick_operator_instance_for_curr_object)
return plan, 'HasSolution'
else:
obstacles_to_remove += objs_in_collision_for_picking_curr_obj
# enumerate through all object orderings
print "Obstacles to remove", obstacles_to_remove
"""
cbefore = []
for oidx, o in enumerate(obstacles_to_remove):
cbefore.append(get_color(o))
set_color(o, [0, 0, float(oidx) / len(obstacles_to_remove)])
[set_color(o, c) for c, o in zip(cbefore, obstacles_to_remove)]
"""
for new_obj_to_move in obstacles_to_remove:
set_color(object_to_move, color_before)
tmp_obstacles_to_remove = set(obstacles_to_remove).difference(
set([new_obj_to_move]))
tmp_obstacles_to_remove = list(tmp_obstacles_to_remove)
#print "tmp obstacles to remove:", tmp_obstacles_to_remove
branch_plan, status = self.search(
new_obj_to_move,
swept_volumes,
tmp_obstacles_to_remove,
objects_moved_before,
plan,
pick_operator_instance_for_curr_object,
parent_obj=object_to_move,
ultimate_plan_stime=ultimate_plan_stime,
timelimit=timelimit)
is_branch_success = status == 'HasSolution'
if is_branch_success:
return branch_plan, status
# It should never come down here, as long as the number of nodes have not exceeded the limit
# but to which level do I back track? To the root node. If this is a root node and
# the number of nodes have not reached the maximum, keep searching.
return False, 'NoSolution'
def __init__(self, problem_env, target_pap):
UnaryPredicate.__init__(self, problem_env)
self.target_pap = target_pap
self.swept_volume = PickAndPlaceSweptVolume(self.problem_env)
self.swept_volume.add_swept_volume(target_pap[0], target_pap[1])