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 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 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 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 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 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 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 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 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 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 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()