def compute_and_cache_ik_solutions(self, ikcachename):
before = CustomStateSaver(self.problem_env.env)
utils.open_gripper()
# for o in self.problem_env.env.GetBodies()[2:]:
# o.Enable(False)
self.problem_env.disable_objects()
self.iksolutions = {}
o = u'c_obst0'
obj = self.problem_env.env.GetKinBody(o)
if True:
# for o, obj in self.objects.items():
# print(o)
self.iksolutions[o] = {r: [] for r in self.regions}
pick_op = Operator(operator_type='one_arm_pick', discrete_parameters={'object': obj})
pick_generator = UniformGenerator(pick_op, self.problem_env)
pick_feasibility_checker = OneArmPickFeasibilityChecker(self.problem_env)
for _ in range(10000):
pick_params = pick_generator.sample_from_uniform()
iks = []
for r, region in self.regions.items():
place_op = Operator(operator_type='one_arm_place',
discrete_parameters={
'object': obj,
'region': region})
place_generator = UniformGenerator(place_op, self.problem_env)
status = 'NoSolution'
for _ in range(10):
obj_pose = place_generator.sample_from_uniform()
set_obj_xytheta(obj_pose, obj)
set_point(obj, np.hstack([obj_pose[0:2], region.z + 0.001]))
params, status = pick_feasibility_checker.check_feasibility(pick_op, pick_params)
if status == 'HasSolution':
iks.append((obj.GetTransform(), params))
break
if status == 'NoSolution':
break
if len(iks) == len(self.regions):
for r, ik in zip(self.regions, iks):
self.iksolutions[o][r].append(ik)
if all(len(self.iksolutions[o][r]) >= 1000 for r in self.regions):
break
# print([len(self.iksolutions[o][r]) for r in self.regions])
self.iksolutions = self.iksolutions[o]
# for o in self.problem_env.env.GetBodies()[2:]:
# o.Enable(True)
self.problem_env.enable_objects()
before.Restore()
pickle.dump(self.iksolutions, open(ikcachename, 'w'))
def is_collision_in_all_volumes(self, object_being_moved):
state_saver = CustomStateSaver(self.problem_env.env)
fold_arms()
for vol in self.swept_volumes:
if self.is_collision_in_single_volume(vol, object_being_moved):
state_saver.Restore()
return True
state_saver.Restore()
return False
def search(self):
# returns the order of objects that respects collision at placements
# todo if I cannot find a grasp or placement in the goal region, then I should declare infeasible problem
init_state = CustomStateSaver(self.problem_env.env)
# self.problem_env.set_exception_objs_when_disabling_objects_in_region(self.goal_objects)
idx = 0
plan = []
goal_obj_move_plan = []
while True:
curr_obj = self.goal_objects[idx]
self.problem_env.disable_objects_in_region('entire_region')
print [o.IsEnabled() for o in self.problem_env.objects]
curr_obj.Enable(True)
pick = self.find_pick(curr_obj)
if pick is None:
plan = [] # reset the whole thing?
goal_obj_move_plan = []
idx += 1
idx = idx % len(self.goal_objects)
init_state.Restore()
print "Pick sampling failed"
continue
pick.execute()
self.problem_env.enable_objects_in_region('entire_region')
place = self.find_place(curr_obj, pick)
if place is None:
plan = []
goal_obj_move_plan = []
idx += 1
idx = idx % len(self.goal_objects)
init_state.Restore()
print "Place sampling failed"
continue
place.execute()
plan.append(pick)
plan.append(place)
goal_obj_move_plan.append(curr_obj)
idx += 1
idx = idx % len(self.goal_objects)
print "Plan length: ", len(plan)
if len(plan) / 2.0 == len(self.goal_objects):
break
init_state.Restore()
return goal_obj_move_plan, plan
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()):
path = [get_body_xytheta(self.problem_env.robot).squeeze()]
#self.reachable_regions_while_holding.append((obj, region_name))
else:
if self.holding_collides is not None:
path, status = motion_planner.get_motion_plan(region, cached_collisions=self.holding_collides)
else:
# I think the trouble here is that we do not hold the object when checking collisions
# So, the best way to solve this problem is to not keep reachable_regions_while_holding
# and just use the cached path. But I am wondering how we got a colliding-path in
# the first place. It must be from place_in_way? No, we execute this function first,
# and then using the cached paths, compute the place_in_way.
# Also, there is something wrong with the collision checking too.
# I think this has to do with the fact that the collisions are computed using
# the robot only, not with the object in hand.
# So, here is what I propose:
# Plan motions here, but do not add to reachable regions while holding.
# This way, it will plan motions as if it is not holding the object,
# but check collisions inside place_in_way
path, status = motion_planner.get_motion_plan(region,
cached_collisions=self.collisions_at_all_obj_pose_pairs)
if status == 'HasSolution':
pass
else:
obj_not_moved = obj != moved_obj
parent_state_has_cached_path_for_obj = parent_state is not None \
and obj in parent_state.cached_place_paths
# todo: What is this logic here...?
# it is about re-using the parent place path;
# but this assumes that the object has not moved?
if parent_state_has_cached_path_for_obj and obj_not_moved:
path = parent_state.cached_place_paths[(obj, region_name)]
else:
path, _ = motion_planner.get_motion_plan(region, cached_collisions={})
saver.Restore()
# assert path is not None
self.cached_place_paths[(obj, region_name)] = path
def get_objects_in_collision_with_given_op_inst(self, op_inst):
saver = CustomStateSaver(self.problem_env.env)
if len(self.problem_env.robot.GetGrabbed()) > 0:
held = self.problem_env.robot.GetGrabbed()[0]
release_obj()
else:
held = None
fold_arms()
new_cols = self.problem_env.get_objs_in_collision(op_inst.low_level_motion, 'entire_region')
saver.Restore()
if held is not None:
grab_obj(held)
return new_cols
def __init__(self, problem_idx, problem_type='normal'):
ProblemEnvironment.__init__(self, problem_idx)
problem_defn = MoverEnvironmentDefinition(self.env)
self.problem_config = problem_defn.get_problem_config()
self.robot = self.env.GetRobots()[0]
self.objects = self.problem_config['packing_boxes']
self.set_problem_type(problem_type)
self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects}
self.initial_robot_base_pose = get_body_xytheta(self.robot)
self.regions = {'entire_region': self.problem_config['entire_region'],
'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']}
self.region_names = ['entire_region', 'home_region', 'loading_region']
self.object_names = [obj.GetName() for obj in self.objects]
self.placement_regions = {'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']}
#self.entity_names = self.object_names + ['home_region', 'loading_region','entire_region']
self.entity_names = self.object_names + ['home_region', 'loading_region', 'entire_region']
self.entity_idx = {name: idx for idx, name in enumerate(self.entity_names)}
self.is_init_pick_node = True
self.name = 'two_arm_mover'
self.init_saver = CustomStateSaver(self.env)
self.problem_config['env'] = self.env
self.operator_names = ['two_arm_pick', 'two_arm_place']
self.reward_function = None
self.applicable_op_constraint = None
self.two_arm_pick_continuous_constraint = None
self.two_arm_place_continuous_constraint = None
self.objects_to_check_collision = None
self.goal = None
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
parent_state_has_cached_path_for_obj \
= parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj
cached_path_is_shortest_path = parent_state is not None and \
not (obj, region_name) in parent_state.reachable_regions_while_holding
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()):
path = [get_body_xytheta(self.problem_env.robot).squeeze()]
self.reachable_regions_while_holding.append((obj, region_name))
else:
if region.name == 'home_region':
# a location right at the entrance of home
goal = [np.array([0.73064842, -2.85306871, 4.87927762])]
else:
goal = region
if self.holding_collides is not None:
path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal,
cached_collisions=self.holding_collides,
return_start_set_and_path_idxs=True)
else:
# note: self.collides is computed without holding the object.
path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal,
cached_collisions=self.collides,
return_start_set_and_path_idxs=True)
if status == 'HasSolution':
self.reachable_regions_while_holding.append((obj, region_name))
else:
if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path:
path = parent_state.cached_place_paths[(obj, region_name)]
start_and_prm_idxs = parent_state.cached_place_start_and_prm_idxs[(obj, region_name)]
else:
path, _, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions={},
return_start_set_and_path_idxs=True)
saver.Restore()
# assert path is not None
self.cached_place_paths[(obj, region_name)] = path
self.cached_place_start_and_prm_idxs[(obj, region_name)] = start_and_prm_idxs
def get_binary_edge_features(self, a, b):
if (a, b) in self.parent_binary_predicates:
return self.parent_binary_predicates[(a, b)]
else:
# todo rename cached_pick_path and cached_place_path as shortest paths
if self.use_shortest_path:
# todo this needs to be re-computed too when we are using shortest paths, because
# this is true if for all b in (a,b,r), b is not in the way of shortest path to r while holidng a
# Since the shortest path plans a path without collision-checking, this is not an accurate computation
if a in self.problem_env.object_names and b in self.problem_env.region_names and b != 'entire_region':
if a not in self.reachable_entities:
is_place_in_b_reachable_while_holding_a = False
else:
saver = CustomStateSaver(self.problem_env.env)
self.pick_used[a].execute(
) # it should not be in collision
motion_planner = BaseMotionPlanner(
self.problem_env, 'prm')
# note that it doesn't check the collision with the object held
plan, status = motion_planner.get_motion_plan(
self.problem_env.regions[b],
cached_collisions=self.collides)
saver.Restore()
is_place_in_b_reachable_while_holding_a = status == 'HasSolution'
else:
is_place_in_b_reachable_while_holding_a = False
else:
is_place_in_b_reachable_while_holding_a = (
a, b) in self.place_in_way.reachable_obj_region_pairs
if self.use_shortest_path:
if b.find('region') != -1:
cached_path = None
else:
cached_path = self.cached_pick_paths[b]
is_a_in_pick_path_of_b = self.pick_in_way(
a, b, cached_path=cached_path)
else:
is_a_in_pick_path_of_b = self.pick_in_way(a, b)
return [
self.in_region(a, b), is_a_in_pick_path_of_b,
is_place_in_b_reachable_while_holding_a
]
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):
# returns the order of objects that respects collision at placements
# todo if I cannot find a grasp or placement in the goal region, then I should declare infeasible problem
init_state = CustomStateSaver(self.problem_env.env)
# self.problem_env.set_exception_objs_when_disabling_objects_in_region(self.goal_objects)
idx = 0
plan = []
goal_obj_move_plan = []
while True:
curr_obj = self.goal_objects[idx]
self.problem_env.disable_objects_in_region('entire_region')
curr_obj.Enable(True)
pap, status = self.find_pick_and_place(curr_obj)
if pap is None:
plan = [] # reset the whole thing?
goal_obj_move_plan = []
idx += 1
idx = idx % len(self.goal_objects)
init_state.Restore()
self.problem_env.objects_to_check_collision = None
print "Pick sampling failed"
continue
pap.execute()
self.problem_env.set_exception_objs_when_disabling_objects_in_region([curr_obj])
print "Pap executed"
plan.append(pap)
goal_obj_move_plan.append(curr_obj)
idx += 1
idx = idx % len(self.goal_objects)
print "Plan length: ", len(plan)
if len(plan) == len(self.goal_objects):
break
init_state.Restore()
self.problem_env.enable_objects_in_region('entire_region')
return goal_obj_move_plan, plan
def sample_feasible_base_pose(self, region):
saver = CustomStateSaver(self.problem_env.env)
domain = get_place_domain(self.problem_env.regions[region])
domain_min = domain[0]
domain_max = domain[1]
in_collision = True
base_pose = None
for i in range(10000):
base_pose = np.random.uniform(domain_min, domain_max, (1, 3)).squeeze()
set_robot_config(base_pose, self.problem_env.robot)
in_collision = self.problem_env.env.CheckCollision(self.problem_env.robot)
if not in_collision:
break
saver.Restore()
if in_collision:
return None
else:
return base_pose
def is_swept_volume_cleared(self, obj):
saver = CustomStateSaver(self.problem_env.env)
if len(self.problem_env.robot.GetGrabbed()) > 0:
held = self.problem_env.robot.GetGrabbed()[0]
release_obj()
else:
held = None
collision_occurred = self.pick_swept_volume.is_collision_in_all_volumes(obj) \
or self.place_swept_volume.is_collision_in_all_volumes(obj)
saver.Restore()
if held is not None:
grab_obj(held)
if collision_occurred:
return False
else:
return True
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
if not self.use_shortest_path:
continue
if pick_path is None:
continue
if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj:
self.cached_place_paths[(
obj, region_name)] = parent_state.cached_place_paths[(
obj, region_name)]
else:
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(
self.problem_env.env.GetKinBody(
obj).ComputeAABB()):
self.cached_place_paths[(obj, region_name)] = [
get_body_xytheta(self.problem_env.robot).squeeze()
]
else:
self.cached_place_paths[(
obj,
region_name)] = motion_planner.get_motion_plan(
region, cached_collisions={})[0]
if self.cached_place_paths[(obj, region_name)] is None:
import pdb
pdb.set_trace()
saver.Restore()
try:
assert self.cached_place_paths[(obj,
region_name)] is not None
except:
import pdb
pdb.set_trace()
def set_cached_place_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for region_name, region in self.problem_env.regions.items():
if region.name == 'entire_region':
continue
for obj, pick_path in self.cached_pick_paths.items():
self.cached_place_paths[(obj, region_name)] = None
parent_state_has_cached_path_for_obj \
= parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj
cached_path_is_shortest_path = parent_state is not None and \
not (obj, region_name) in parent_state.reachable_regions_while_holding
saver = CustomStateSaver(self.problem_env.env)
pick_used = self.pick_used[obj]
pick_used.execute()
if region.contains(
self.problem_env.env.GetKinBody(obj).ComputeAABB()):
path = [get_body_xytheta(self.problem_env.robot).squeeze()]
self.reachable_regions_while_holding.append(
(obj, region_name))
else:
if self.holding_collides is not None:
path, status = motion_planner.get_motion_plan(
region, cached_collisions=self.holding_collides)
else:
path, status = motion_planner.get_motion_plan(
region, cached_collisions=self.collides)
if status == 'HasSolution':
self.reachable_regions_while_holding.append(
(obj, region_name))
else:
if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path:
path = parent_state.cached_place_paths[(
obj, region_name)]
else:
path, _ = motion_planner.get_motion_plan(
region, cached_collisions={})
saver.Restore()
# assert path is not None
self.cached_place_paths[(obj, region_name)] = path
def search(self, start_time, time_limit):
# returns the order of objects that respects collision at placements
# todo if I cannot find a grasp or placement in the goal region, then I should declare infeasible problem
init_state = CustomStateSaver(self.problem_env.env)
# self.problem_env.set_exception_objs_when_disabling_objects_in_region(self.goal_objects)
idx = 0
plan = []
goal_obj_move_plan = []
while True:
if time.time() - start_time > time_limit:
return None, None
curr_obj = self.goal_objects[idx]
self.problem_env.disable_objects_in_region('entire_region')
print[o.IsEnabled() for o in self.problem_env.objects]
curr_obj.Enable(True)
pap = self.find_pap(curr_obj)
if pap is None:
plan = [] # reset the whole thing?
goal_obj_move_plan = []
idx += 1
idx = idx % len(self.goal_objects)
init_state.Restore()
print "Pap sampling failed"
continue
pap.execute()
plan.append(pap)
goal_obj_move_plan.append(curr_obj)
idx += 1
idx = idx % len(self.goal_objects)
print "Plan length: ", len(plan)
if len(plan) == len(self.goal_objects):
break
init_state.Restore()
return goal_obj_move_plan, plan
def determine_collision_and_collision_free_picks(self):
for obj, params in self.pick_params.items():
for pp in params:
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pp
)
before = CustomStateSaver(self.problem_env.env)
pick_op.execute()
if not self.problem_env.env.CheckCollision(self.problem_env.robot):
self.nocollision_pick_op[obj] = pick_op
before.Restore()
break
collisions = {
o for o in self.objects
if self.problem_env.env.CheckCollision(self.problem_env.env.GetKinBody(o))
}
if obj not in self.collision_pick_op or len(collisions) < len(self.collision_pick_op[obj][1]):
self.collision_pick_op[obj] = pick_op, collisions
before.Restore()
def count_collides(papp):
before = CustomStateSaver(self.problem_env.env)
pickp, placep = papp
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pickp
)
place_op = Operator(
operator_type='one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]},
continuous_parameters=placep
)
pick_op.execute()
pick_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
place_op.execute()
place_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
before.Restore()
return pick_collisions + place_collisions
def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None):
self.state_saver = CustomStateSaver(problem_env.env)
self.problem_env = problem_env
self.parent = parent_state # used to update the node features
# raw variables
self.robot_pose = get_body_xytheta(problem_env.robot)
self.object_poses = {
obj.GetName(): get_body_xytheta(obj)
for obj in problem_env.objects
}
def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None,
paps_used_in_data=None):
self.state_saver = CustomStateSaver(problem_env.env)
self.problem_env = problem_env
self.problem_env.set_robot_to_default_dof_values()
self.parent_state = parent_state # used to update the node features
self.goal_entities = goal_entities
self.object_names = [str(obj.GetName()) for obj in problem_env.objects]
# raw variables
self.robot_pose = get_body_xytheta(problem_env.robot)
self.object_poses = {
obj.GetName(): get_body_xytheta(obj)
for obj in problem_env.objects
}
self.use_prm = problem_env.name.find('two_arm') != -1
if paps_used_in_data is not None:
self.pick_used = paps_used_in_data[0]
self.place_used = paps_used_in_data[1]
self.mc_pick_path = {}
self.mc_place_path = {}
self.reachable_entities = []
self.pick_in_way = None
self.place_in_way = None
self.in_region = None
self.is_holding_goal_entity = None
self.ternary_edges = None
self.binary_edges = None
self.nodes = None
self.prm_vertices, self.prm_edges = pickle.load(open(
'./prm.pkl', 'rb'))
def __init__(self):
ProblemEnvironment.__init__(self)
problem = MoverProblem(self.env)
self.problem_config = problem.get_problem_config()
self.robot = self.env.GetRobots()[0]
self.objects = self.problem_config['packing_boxes']
self.object_init_poses = {
o.GetName(): get_body_xytheta(o).squeeze()
for o in self.objects
}
self.init_base_conf = np.array([0, 1.05, 0])
self.regions = {
'entire_region': self.problem_config['entire_region'],
'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']
}
self.placement_regions = {
'home_region': self.problem_config['home_region'],
'loading_region': self.problem_config['loading_region']
}
self.entity_names = [obj.GetName()
for obj in self.objects] + list(self.regions)
self.entity_idx = {
name: idx
for idx, name in enumerate(self.entity_names)
}
self.is_init_pick_node = True
self.name = 'mover'
self.init_saver = CustomStateSaver(self.env)
self.problem_config['env'] = self.env
self.operator_names = ['two_arm_pick', 'two_arm_place']
self.reward_function = None
self.applicable_op_constraint = None
self.two_arm_pick_continuous_constraint = None
self.two_arm_place_continuous_constraint = None
self.objects_to_check_collision = None
def determine_collision_and_collision_free_places(self):
before = CustomStateSaver(self.problem_env.env)
for obj in self.objects:
for r in self.regions:
# print obj
if len(self.pap_params[(obj, r)]) > 0:
def count_collides(papp):
before = CustomStateSaver(self.problem_env.env)
pickp, placep = papp
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pickp
)
place_op = Operator(
operator_type='one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]},
continuous_parameters=placep
)
pick_op.execute()
pick_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
place_op.execute()
place_collisions = sum(self.problem_env.env.CheckCollision(o) for o in self.problem_env.objects)
before.Restore()
return pick_collisions + place_collisions
# chooses the one with minimal collisions
papp = min(self.pap_params[(obj, r)], key=lambda papp: count_collides(papp))
pickp, placep = papp
pick_op = Operator(
operator_type='one_arm_pick',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj)},
continuous_parameters=pickp
)
pick_op.execute()
# pap gets placed in nocollision if both pick and place have no collision
# otherwise it gets placed in collision, along with objects that collide with the place config (but not the pick config)
pick_collision = self.problem_env.env.CheckCollision(self.problem_env.robot)
place_op = Operator(
operator_type='one_arm_place',
discrete_parameters={'object': self.problem_env.env.GetKinBody(obj),
'region': self.problem_env.regions[r]},
continuous_parameters=placep
)
place_op.execute()
place_collision = self.problem_env.env.CheckCollision(self.problem_env.robot) \
or self.problem_env.env.CheckCollision(self.problem_env.env.GetKinBody(obj))
if not pick_collision and not place_collision:
self.nocollision_place_op[(obj, r)] = pick_op, place_op
# if obj in self.goal_entities and r in self.goal_entities:
# print('successful goal pap')
before.Restore()
continue
collisions = {
o for o in self.objects
if self.problem_env.env.CheckCollision(self.problem_env.env.GetKinBody(o))
}
if (obj, r) not in self.collision_place_op or len(collisions) < \
len(self.collision_place_op[(obj, r)][1]):
self.collision_place_op[(obj, r)] = place_op, collisions
before.Restore()
def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None,
paps_used_in_data=None,
use_shortest_path=False):
self.state_saver = CustomStateSaver(problem_env.env)
self.problem_env = problem_env
self.parent_state = parent_state
self.goal_entities = goal_entities
# raw variables
self.robot_pose = get_body_xytheta(problem_env.robot)
self.object_poses = {
obj.GetName(): get_body_xytheta(obj)
for obj in problem_env.objects
}
# cached info
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
# adopt from parent predicate evaluations
self.parent_ternary_predicates = {}
self.parent_binary_predicates = {}
if parent_state is not None and paps_used_in_data is None:
assert parent_action 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.parent_ternary_predicates = {
(a, b, r): v
for (a, b, r), v in parent_state.ternary_edges.items()
if a != moved_obj and b != moved_obj
}
self.parent_binary_predicates = {
(a, b): v
for (a, b), v in parent_state.binary_edges.items()
if a != moved_obj and b != moved_obj
}
self.use_shortest_path = False
self.cached_place_paths = {}
if paps_used_in_data is None:
self.pick_used = {}
self.place_used = {}
else:
self.pick_used = paps_used_in_data[0]
self.place_used = paps_used_in_data[1]
self.mc_pick_path = {}
self.mc_place_path = {}
self.reachable_entities = []
# predicates
self.pick_in_way = PickInWay(self.problem_env,
collides=self.current_collides,
pick_poses=self.pick_used,
use_shortest_path=self.use_shortest_path)
self.place_in_way = PlaceInWay(
self.problem_env,
collides=self.current_collides,
pick_poses=self.pick_used,
use_shortest_path=self.use_shortest_path)
self.in_region = InRegion(self.problem_env)
self.is_holding_goal_entity = IsHoldingGoalEntity(
self.problem_env, goal_entities)
self.ternary_edges = self.get_ternary_edges()
self.binary_edges = self.get_binary_edges()
self.nodes = self.get_nodes()
def set_goal(self, goal_objects, goal_region):
self.goal_objects = goal_objects
[utils.set_color(o, [1, 0, 0]) for o in self.goal_objects]
if 40000 <= self.problem_idx < 50000:
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance and obj.GetName() not in goal_objects][0:3]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
# surround the robot?
# Force the goal object to be around the robot
#utils.randomly_place_region(self.robot, robot_region)
radius = 1
center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
xmin = center[0] - radius
xmax = center[0]
ymin = center[1]
ymax = center[1] + radius
goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# doing the below because we ran n_objs_pack=1 and n_objs_pack=4 on different commits
if len(goal_objects) == 4:
for obj in goal_objects[0:1]:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
else:
for obj in goal_objects:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region, n_limit=100)
elif 50000 <= self.problem_idx < 60000:
# hard problems for both RSC and Greedy
# hard for RSC: make sure the goal object needs to be moved twice
# dillema: if I put the goal object near the entrance, then I can just move that to the goal region
# I must surround the robot with the goal object such that the shortest path always go
# through the goal object
# another option is to block the object in the entrance region with the goal object
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region',
# xmin,xmax ymin, ymax
((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance
if obj.GetName() not in goal_objects][0:3]
# object_around_entrance = np.array(object_around_entrance)[
# np.random.choice(range(len(object_around_entrance)), 3, replace=False)]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
# xmin,xmax ymin, ymax
robot_region = AARegion('robot_region', ((3.0, 4.29), (-8.0, -6.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
utils.randomly_place_region(self.robot, robot_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects
if obj not in object_around_entrance + objs_to_move_near_entrance]
# surround the robot?
# Force the goal object to be around the robot
utils.randomly_place_region(self.robot, robot_region)
radius = 1
center = utils.get_body_xytheta(self.robot).squeeze()[0:2]
xmin = center[0] - radius
xmax = center[0] + radius
ymin = center[1] - radius
ymax = center[1] + radius
goal_obj_region = AARegion('goal_obj_region', ((xmin, xmax), (ymin, ymax)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
for obj in goal_objects:
utils.randomly_place_region(self.env.GetKinBody(obj), goal_obj_region)
elif self.problem_idx >= 60000:
entrance_region = AARegion('entrance', ((0.25, 1.33), (-6, -5.0)), z=0.135, color=np.array((1, 1, 0, 0.25)))
non_entrance_region = AARegion('non_entrance_region', ((1.5, 4.25), (-8.49, -5.01)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
# move objects out of the entrance region
utils.randomly_place_region(self.robot, non_entrance_region)
[utils.randomly_place_region(obj, non_entrance_region) for obj in self.objects]
# try to put three objs near the entrance
objs_to_move_near_entrance = [obj for obj in self.objects if obj.GetName() not in goal_objects][0:1]
for obj in objs_to_move_near_entrance:
utils.randomly_place_region(obj, entrance_region, n_limit=100)
region_around_entrance_region = AARegion('region_around', ((-0.25, 1.7), (-6.6, -5.0)), z=0.135,
color=np.array((1, 1, 0, 0.25)))
object_around_entrance = [obj for obj in self.objects if obj not in objs_to_move_near_entrance][0:3]
for obj in object_around_entrance: utils.randomly_place_region(obj, region_around_entrance_region,
n_limit=100)
self.initial_robot_base_pose = get_body_xytheta(self.robot)
self.object_init_poses = {o.GetName(): get_body_xytheta(o).squeeze() for o in self.objects}
self.init_saver = CustomStateSaver(self.env)
self.goal_region = goal_region
self.goal_entities = self.goal_objects + [self.goal_region]
