def get_objects_in_collision_with_pick_and_place(self, parent_obj_pick,
place):
saver = utils.CustomStateSaver(self.problem_env.env)
place_collisions \
= self.place_swept_volume.get_objects_in_collision_with_given_op_inst(place)
# place the object at the desired place
associated_pick = self.place_swept_volume.pick_used[place]
associated_pick.execute()
place.execute()
# utils.two_arm_pick_object(associated_pick.discrete_parameters['object'], associated_pick.continuous_parameters)
# utils.two_arm_place_object(place.continuous_parameters)
# now check the collision to the parent object
if parent_obj_pick is not None:
# how could I collide with obj to pick here?
pick_collisions \
= self.pick_swept_volume.get_objects_in_collision_with_given_op_inst(parent_obj_pick)
else:
pick_collisions = []
saver.Restore()
objs_in_collision = place_collisions + [
o for o in pick_collisions if not (o in place_collisions)
]
return objs_in_collision
def __init__(self, problem_idx):
Mover.__init__(self, problem_idx)
self.operator_names = ['one_arm_pick', 'one_arm_place']
set_robot_config([4.19855789, 2.3236321, 5.2933337], self.robot)
set_obj_xytheta([3.35744004, 2.19644156, 3.52741118], self.objects[1])
self.boxes = self.objects
self.objects = self.problem_config['shelf_objects']
self.objects = [k for v in self.objects.values() for k in v]
self.objects[0], self.objects[1] = self.objects[1], self.objects[0]
self.target_box = self.env.GetKinBody('rectangular_packing_box1')
utils.randomly_place_region(self.target_box, self.regions['home_region'])
self.regions['rectangular_packing_box1_region'] = self.compute_box_region(self.target_box)
self.shelf_regions = self.problem_config['shelf_regions']
self.target_box_region = self.regions['rectangular_packing_box1_region']
self.regions.update(self.shelf_regions)
self.entity_names = [obj.GetName() for obj in self.objects] + ['rectangular_packing_box1_region',
'center_shelf_region']
self.name = 'one_arm_mover'
self.init_saver = utils.CustomStateSaver(self.env)
self.object_names = self.entity_names
# fix incorrectly named regions
self.regions = {
region.name: region
for region in self.regions.values()
}
def create_node(self,
parent_action,
depth,
parent_node,
is_parent_action_infeasible,
is_init_node=False):
state_saver = utils.CustomStateSaver(self.problem_env.env)
is_operator_skeleton_node = (parent_node is None) or (
not parent_node.is_operator_skeleton_node)
state = self.get_current_state(parent_node, parent_action,
is_parent_action_infeasible)
if is_operator_skeleton_node:
applicable_op_skeletons = self.problem_env.get_applicable_ops(
parent_action)
node = DiscreteTreeNodeWithPriorQ(state, self.ucb_parameter, depth,
state_saver,
is_operator_skeleton_node,
is_init_node,
applicable_op_skeletons,
self.learned_q)
else:
node = ContinuousTreeNode(state, parent_action, self.ucb_parameter,
depth, state_saver,
is_operator_skeleton_node, is_init_node)
node.sampling_agent = self.create_sampling_agent(node)
node.parent = parent_node
node.parent_action = parent_action
return node
def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None,
planner='greedy'):
PaPState.__init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None,
paps_used_in_data=None)
self.parent_state = parent_state
self.parent_ternary_predicates = {}
self.parent_binary_predicates = {}
self.goal_entities = goal_entities
if parent_state is not None:
moved_obj_type = type(
parent_action.discrete_parameters['two_arm_place_object'])
if moved_obj_type == str or moved_obj_type == unicode:
moved_obj = parent_action.discrete_parameters[
'two_arm_place_object']
else:
moved_obj = parent_action.discrete_parameters[
'two_arm_place_object'].GetName()
self.initialize_parent_predicates(moved_obj, parent_state,
parent_action)
else:
moved_obj = None
problem_env.enable_objects_in_region('entire_region')
self.reachable_entities = []
self.reachable_regions_while_holding = []
self.pick_used = {
object.GetName(): self.get_pick_poses(object, moved_obj,
parent_state)
for object in problem_env.objects
}
if self.use_prm:
if parent_state is None:
self.collides, self.current_collides = self.update_collisions_at_prm_vertices(
None)
else:
self.collides, self.current_collides = self.update_collisions_at_prm_vertices(
parent_state.collides)
self.holding_collides = None
self.current_holding_collides = None
# hold an object and check collisions
if planner == 'mcts':
self.holding_collides = None
self.current_holding_collides = None
saver = utils.CustomStateSaver(self.problem_env.env)
self.pick_used.values()[0].execute()
if parent_state is None:
self.holding_collides, self.current_holding_collides = self.update_collisions_at_prm_vertices(
None)
else:
self.holding_collides, self.current_holding_collides \
= self.update_collisions_at_prm_vertices(parent_state.holding_collides)
# Check if, while holding, this config is in collision: np.array([ 1.13287863, -4.72498756, -2.53161845])
# target_q = np.array([1.13287863, -4.72498756, -2.53161845])
saver.Restore()
else:
self.holding_collides = None
self.holding_current_collides = None
self.key_config_obstacles = self.make_key_config_obstacles_from_prm_collisions(
)
self.place_used = {}
self.cached_pick_paths = {}
self.cached_place_paths = {}
self.set_cached_pick_paths(parent_state, moved_obj)
self.set_cached_place_paths(parent_state, moved_obj)
# predicates
self.pick_in_way = PickInWay(self.problem_env,
collides=self.current_collides,
pick_poses=self.pick_used,
use_shortest_path=True)
self.place_in_way = PlaceInWay(self.problem_env,
collides=self.current_collides,
pick_poses=self.pick_used,
use_shortest_path=True)
self.in_region = InRegion(self.problem_env)
self.is_holding_goal_entity = IsHoldingGoalEntity(
self.problem_env, goal_entities)
self.nodes = self.get_nodes()
self.binary_edges = self.get_binary_edges()
self.ternary_edges = self.get_ternary_edges()
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'
