def set_cached_pick_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
# making it consistent with the feasibility checker
self.problem_env.set_robot_to_default_dof_values()
for obj, op_instance in self.pick_used.items():
# print self.problem_env.robot.GetDOFValues()
motion_plan_goals = op_instance.continuous_parameters['q_goal']
assert len(motion_plan_goals) > 0
self.cached_pick_paths[obj] = None
path, status = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions=self.collides)
if status == 'HasSolution':
self.reachable_entities.append(obj)
else:
path, _ = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions={})
[t.Enable(False) for t in self.problem_env.objects]
rrt_motion_planner = BaseMotionPlanner(self.problem_env, 'rrt')
# motion_plan_goals[0] = np.array([-0.11255534, -0.26290062, 1.64126379])
# utils.set_robot_config(motion_plan_goals[0])
for _ in range(100):
path, status = rrt_motion_planner.get_motion_plan(motion_plan_goals[0])
if status == 'HasSolution':
break
[t.Enable(True) for t in self.problem_env.objects]
assert path is not None, 'Even RRT failed!'
self.cached_pick_paths[obj] = path
op_instance.low_level_motion = path
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 set_cached_pick_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for obj, op_instance in self.pick_used.items():
motion_plan_goals = op_instance.continuous_parameters['q_goal']
self.cached_pick_paths[obj] = None
if not self.use_shortest_path:
continue
if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj:
self.cached_pick_paths[obj] = parent_state.cached_pick_paths[
obj]
else:
self.cached_pick_paths[obj] = motion_planner.get_motion_plan(
motion_plan_goals, cached_collisions={})[0]
if len(motion_plan_goals) == 0:
# This pick path is used for checking the pickinway predicate and to pickup the object in place in way predicate.
#
assert False
self.cached_pick_paths[obj] = None
else:
try:
# how can this be, since we disable all the collisions?
assert self.cached_pick_paths[obj] is not None
except:
import pdb
pdb.set_trace()
op_instance.continuous_parameters[
'potential_q_goals'] = motion_plan_goals
op_instance.continuous_parameters[
'q_goal'] = self.cached_pick_paths[obj][-1]
def approximate_minimal_collision_path(self, goal_configuration, path_ignoring_all_objects,
collisions_in_path_ignoring_all_objects, cached_collisions):
enabled_objects = {obj.GetName() for obj in self.problem_env.objects}
enabled_objects -= {obj.GetName() for obj in collisions_in_path_ignoring_all_objects}
[o.Enable(False) for o in collisions_in_path_ignoring_all_objects]
minimal_objects_in_way = []
minimal_collision_path = path_ignoring_all_objects
for obj in collisions_in_path_ignoring_all_objects:
obj.Enable(True)
[o.Enable(False) for o in minimal_objects_in_way]
enabled_objects.add(obj.GetName())
enabled_objects -= {obj.GetName() for obj in minimal_objects_in_way}
if self.problem_env.name.find('one_arm') != -1:
path, status = ArmBaseMotionPlanner.get_motion_plan(self, goal_configuration,
cached_collisions=cached_collisions)
else:
path, status = BaseMotionPlanner.get_motion_plan(self,
goal_configuration,
cached_collisions=cached_collisions,
n_iterations=[20, 50, 100])
if status != 'HasSolution':
minimal_objects_in_way.append(obj)
else:
minimal_collision_path = path
self.problem_env.enable_objects_in_region('entire_region')
return minimal_collision_path
def check_obj_reachable(self, obj):
if len(self.problem_env.robot.GetGrabbed()) > 0:
return False
obj = self.problem_env.env.GetKinBody(obj)
obj_name = str(obj.GetName())
if self.problem_env.name.find('one_arm') != -1:
op = Operator('one_arm_pick', {'object': obj})
else:
op = Operator('two_arm_pick', {'object': obj})
if obj_name in self.pick_used:
motion_plan_goals = self.pick_used[obj_name].continuous_parameters['q_goal']
else:
motion_plan_goals = self.generate_potential_pick_configs(op, n_pick_configs=10)
if motion_plan_goals is not None:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
motion, status = motion_planner.get_motion_plan(motion_plan_goals)
is_feasible_param = status == 'HasSolution'
else:
is_feasible_param = False
if is_feasible_param:
op.make_pklable()
op.continuous_parameters = {'q_goal': motion[-1]}
self.motion_plans[obj_name] = motion
if obj_name not in self.pick_used:
self.pick_used[obj_name] = op
self.evaluations[obj_name] = True
return True
else:
self.evaluations[obj_name] = False
return False
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 set_cached_pick_paths(self, parent_state, moved_obj):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
for obj, op_instance in self.pick_used.items():
motion_plan_goals = op_instance.continuous_parameters['q_goal']
assert len(motion_plan_goals) > 0
self.cached_pick_paths[obj] = None
path, status = motion_planner.get_motion_plan(
motion_plan_goals, cached_collisions=self.collides)
if status == 'HasSolution':
self.reachable_entities.append(obj)
else:
path, _ = motion_planner.get_motion_plan(motion_plan_goals,
cached_collisions={})
assert path is not None
self.cached_pick_paths[obj] = path
op_instance.low_level_motion = path
def get_minimum_constraint_path_to(self, goal_config, target_obj):
print "Planning to goal config:", goal_config
if self.use_shortest_path:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
else:
motion_planner = MinimumConstraintPlanner(self.problem_env, target_obj, 'prm')
motion, status = motion_planner.get_motion_plan(goal_config, cached_collisions=self.collides)
if motion is None:
return None, 'NoSolution'
return motion, status
def two_arm_domain_region_reachability_check(self, region):
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
print "Motion planning to ", region
motion, status = motion_planner.get_motion_plan(self.problem_env.regions[region],
cached_collisions=self.collides)
if status == 'HasSolution':
self.motion_plans[region] = motion
self.evaluations[region] = True
return True
else:
self.evaluations[region] = False
return False
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 compute_path_ignoring_obstacles(self, goal_configuration):
self.problem_env.disable_objects_in_region('entire_region')
if self.target_object is not None:
self.target_object.Enable(True)
if self.problem_env.name.find('one_arm') != -1:
path, status = ArmBaseMotionPlanner.get_motion_plan(self, goal_configuration)
else:
stime = time.time()
path, status = BaseMotionPlanner.get_motion_plan(self, goal_configuration)
print "Motion plan time", time.time()-stime
self.problem_env.enable_objects_in_region('entire_region')
if path is None:
import pdb; pdb.set_trace()
return path
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 approximate_minimal_collision_path(
self, goal_configuration, path_ignoring_all_objects,
collisions_in_path_ignoring_all_objects, cached_collisions):
# enabled objects = all the objects except the ones that are in collision
enabled_objects = {obj.GetName() for obj in self.problem_env.objects}
enabled_objects -= {
obj.GetName()
for obj in collisions_in_path_ignoring_all_objects
}
[o.Enable(False) for o in collisions_in_path_ignoring_all_objects]
# enable object one by one. If we can find a path with it turned off, then it is not actually in the way.
# minimal objects in way = a set of objects that we cannot ignore
# for other objects in collisions_in_path_ignoring_all_objects, we can ignore
minimal_objects_in_way = []
minimal_collision_path = path_ignoring_all_objects
print "Approximating MCR path..."
for obj in collisions_in_path_ignoring_all_objects:
obj.Enable(True)
[o.Enable(False) for o in minimal_objects_in_way]
enabled_objects.add(obj.GetName())
enabled_objects -= {
obj.GetName()
for obj in minimal_objects_in_way
}
if self.problem_env.name.find('one_arm') != -1:
path, status = ArmBaseMotionPlanner.get_motion_plan(
self,
goal_configuration,
cached_collisions=cached_collisions)
else:
path, status = BaseMotionPlanner.get_motion_plan(
self,
goal_configuration,
cached_collisions=cached_collisions,
n_iterations=[20, 50, 100, 500, 1000])
if status != 'HasSolution':
minimal_objects_in_way.append(obj)
else:
minimal_collision_path = path
self.problem_env.enable_objects_in_region('entire_region')
return minimal_collision_path
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 get_node_features(self, entity, goal_entities):
isobj = entity not in self.problem_env.regions
obj = self.problem_env.env.GetKinBody(entity) if isobj else None
pose = get_body_xytheta(obj)[0] if isobj else None
if isobj:
if self.use_shortest_path:
motion_planner = BaseMotionPlanner(self.problem_env, 'prm')
pick_for_obj = self.pick_used[obj.GetName()]
plan, status = motion_planner.get_motion_plan(
pick_for_obj.continuous_parameters['potential_q_goals'],
cached_collisions=self.collides)
pick_for_obj.low_level_motion = plan
if status == 'HasSolution':
pick_for_obj.continuous_parameters['q_goal'] = plan[-1]
self.reachable_entities.append(entity)
is_entity_reachable = True
else:
is_entity_reachable = False
else:
is_entity_reachable = obj.GetName() in self.reachable_entities
else:
is_entity_reachable = True
return [
0, # l
0, # w
0, # h
pose[0] if isobj else 0, # x
pose[1] if isobj else 0, # y
pose[2] if isobj else 0, # theta
entity not in self.problem_env.regions, # IsObj
entity in self.problem_env.regions, # IsRoom
entity in self.goal_entities, # IsGoal
is_entity_reachable,
self.is_holding_goal_entity(),
]