def create_environment(self):
problem_env = Mover(self.problem_idx)
openrave_env = problem_env.env
target_object = openrave_env.GetKinBody('square_packing_box1')
set_color(target_object, [1, 0, 0])
return problem_env, openrave_env
def centered_box_body(env, dx, dy, dz, name=None, color=None, transparency=None): body = RaveCreateKinBody(env, '') body.InitFromBoxes(np.array([[0, 0, 0, .5*dx, .5*dy, .5*dz]]), draw=True) if name is not None: set_name(body, name) if color is not None: set_color(body, color) if transparency is not None: set_transparency(body, transparency) return body
def add_trajectory(self, plan, goal_entities):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
motion_planner = BaseMotionPlanner(problem_env, 'prm')
problem_env.set_motion_planner(motion_planner)
idx = 0
parent_state = None
parent_action = None
self.plan_sanity_check(problem_env, plan)
paps_used = self.get_pap_used_in_plan(plan)
pick_used = paps_used[0]
place_used = paps_used[1]
reward_function = ShapedRewardFunction(problem_env,
['square_packing_box1'],
'home_region', 3 * 8)
# utils.viewer()
state = self.compute_state(parent_state, parent_action, goal_entities,
problem_env, paps_used, 0)
for action_idx, action in enumerate(plan):
if 'place' in action.type:
continue
target_obj = openrave_env.GetKinBody(
action.discrete_parameters['object'])
color_before = get_color(target_obj)
set_color(target_obj, [1, 1, 1])
pick_used, place_used = self.delete_moved_objects_from_pap_data(
pick_used, place_used, target_obj)
paps_used = [pick_used, place_used]
action.is_skeleton = False
pap_action = copy.deepcopy(action)
pap_action = pap_action.merge_operators(plan[action_idx + 1])
pap_action.is_skeleton = False
pap_action.execute()
# set_color(target_obj, color_before)
parent_state = state
parent_action = pap_action
state = self.compute_state(parent_state, parent_action,
goal_entities, problem_env, paps_used,
action_idx)
# execute the pap action
reward = reward_function(parent_state, state, parent_action,
action_idx)
print "The reward is ", reward
self.add_sar_tuples(parent_state, pap_action, reward)
print "Executed", action.discrete_parameters
self.add_state_prime()
openrave_env.Destroy()
openravepy.RaveDestroy()
def visualize_values_in_two_arm_domains(self, entity_values, entity_names):
is_place_node = entity_names[0].find('region') != -1
if is_place_node:
return
max_val = np.max(entity_values)
min_val = np.min(entity_values)
for entity_name, entity_value in zip(entity_names, entity_values):
entity = openravepy.RaveGetEnvironments()[0].GetKinBody(entity_name)
set_color(entity, [0, (entity_value - min_val) / (max_val - min_val), 0])
def assert_region(region):
try:
assert self.problem_env.get_region_containing(self.target_obj).name == region.name
return True
except Exception as e:
print(e)
set_color(self.target_obj, [1,1,1])
import pdb
pdb.set_trace()
return False
def create_environment(self):
problem_env = PaPMoverEnv(self.problem_idx)
openrave_env = problem_env.env
goal_objs = ['square_packing_box1']
goal_region = 'home_region'
problem_env.set_goal(goal_objs, goal_region)
target_object = openrave_env.GetKinBody('square_packing_box1')
set_color(target_object, [1, 0, 0])
return problem_env, openrave_env
def visualize_q_values(self, problem_env):
traj = pickle.load(
open('./test_results/mcts_results_on_mover_domain/widening_5/uct_1.0/raw_data/traj_pidx_0.pkl', 'r'))
state = traj.states[0]
action = traj.actions[0]
print "Truth: ", action.discrete_parameters['object']
q_function = self.load_qinit(problem_env.entity_idx)
objects = problem_env.objects
object_names = [str(p.GetName()) for p in objects]
other_actions = [copy.deepcopy(action) for a in object_names]
for a, obj_name in zip(other_actions, object_names):
a.discrete_parameters['object'] = obj_name
pred = q_function.predict(state, a)
print pred, a.discrete_parameters
obj = problem_env.env.GetKinBody(obj_name)
set_color(obj, [0, 0, 1.0 / float(pred)])
def visualize_place_inways(self):
self.problem_env.env.SetViewer('qtcoin')
for key, val in self.place_in_way.mc_path_to_entity.items():
hold_obj_name = key[0]
region_name = key[1]
objs_in_way = self.place_in_way.mc_to_entity[key]
if len(objs_in_way) > 0:
saver = CustomStateSaver(self.problem_env.env)
self.pick_used[hold_obj_name].execute()
for tmp in objs_in_way:
set_color(self.problem_env.env.GetKinBody(tmp), [0, 0, 0])
visualize_path(val)
import pdb
pdb.set_trace()
for tmp in objs_in_way:
set_color(self.problem_env.env.GetKinBody(tmp), [0, 1, 0])
saver.Restore()
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,
goal_entities,
parent_state=None,
parent_action=None):
AbstractState.__init__(problem_env, goal_entities, parent_state,
parent_action)
self.use_prm = problem_env.name.find('two_arm') != -1
if self.use_prm:
self.collides, self.current_collides = self.update_collisions_at_prm_vertices(
parent_state)
else:
self.collides = None
self.current_collides = None
# predicates
self.is_reachable = IsReachable(self.problem_env,
collides=self.current_collides)
self.in_way = InWay(self.problem_env, collides=self.current_collides)
self.in_region = InRegion(self.problem_env)
self.is_holding_goal_entity = IsHoldingGoalEntity(
self.problem_env, goal_entities)
# GNN todo write this in a separate function
self.goal_entities = goal_entities
if parent_state is not None:
self.nodes = {}
self.edges = {}
if parent_action.type.find('pick') != -1:
self.update_node_features(parent_state)
self.update_edge_features(parent_state)
elif parent_action.type.find('place') != -1:
assert len(self.problem_env.robot.GetGrabbed()) == 0
grand_parent_state = parent_state.parent
self.update_node_features(
grand_parent_state,
parent_action.discrete_parameters['object'])
self.update_edge_features(
grand_parent_state,
parent_action.discrete_parameters['object'])
else:
raise NotImplementedError
else:
self.nodes = self.get_nodes()
self.edges = self.get_edges()
self.update_reachability_based_on_inway()
# for debugging purpose; to be deleted later
reachable_idx = 9
self.reachable_entities = [
n for n in self.nodes if self.nodes[n][reachable_idx]
]
for entity in self.nodes:
if entity.find('region') != -1:
continue
if self.nodes[entity][reachable_idx]:
set_color(self.problem_env.env.GetKinBody(entity), [1, 1, 1])
else:
set_color(self.problem_env.env.GetKinBody(entity), [0, 0, 0])
self.print_reachable_entities()
self.print_inway_entities()
# if the goal object is not reachable with naive path, then declare infeasible problem
goal_object = self.goal_entities[0]
objects_with_mc_path = self.in_way.minimum_constraint_path_to_entity.keys(
)
is_pick_state = len(self.problem_env.robot.GetGrabbed()) == 0
if is_pick_state and not (goal_object
in self.is_reachable.reachable_entities
or goal_object in objects_with_mc_path):
print "Infeasible problem instance"
