def __init__(self, problem_env, goal_entities, parent_state=None, parent_action=None, paps_used_in_data=None):
PaPState.__init__(self, problem_env, goal_entities, parent_state=None, parent_action=None)
self.name = 'one_arm_mover'
self.objects = objects = {
o: problem_env.env.GetKinBody(o)
for o in problem_env.entity_names
if 'region' not in o
}
self.regions = regions = {
r: problem_env.regions[r]
for r in problem_env.entity_names
if 'region' in r
}
self.object_names = [o for o in problem_env.entity_names if 'region' not in o]
self.region_names = [o for o in problem_env.entity_names if 'region' in o]
# cache ik solutions
ikcachename = './ikcache.pkl'
import collections
if parent_state is not None:
self.iksolutions = parent_state.iksolutions
elif os.path.isfile(ikcachename):
print "Loading ik cache from harddrive"
self.iksolutions = pickle.load(open(ikcachename, 'r'))
else:
# self.compute_and_cache_ik_solutions(ikcachename)
self.iksolutions = collections.defaultdict(list)
# ik solutions contain 1000 paps.
# Suppose at each state, we try 15 ik attempts for each object (the number we have for non-goal-entities).
# We have 10 objects
# On failure IK, it takes about 0.15 seconds
# On successful ones, it takes close-to-zero seconds. Say on average it succeeds half of the times.
# Average IK time is then 0.075
# 15*10*0.075 = 11.25 seconds.
# But we check collisions with cached IKs, which take about 4-5 seconds on average. Say it takes 4.5s.
# So this adds to about 6.25.
self.pick_used = {}
self.place_used = {}
self.parent_state = parent_state
self.parent_ternary_predicates = {}
self.parent_binary_predicates = {}
if parent_state is not None:
moved_obj_type = type(parent_action.discrete_parameters['object'])
if moved_obj_type == str or moved_obj_type == unicode:
moved_obj = parent_action.discrete_parameters['object']
else:
moved_obj = parent_action.discrete_parameters['object'].GetName()
else:
moved_obj = None
init_robot_config = utils.get_rightarm_torso_config(self.problem_env.robot)
self.pap_params = {}
self.pick_params = {}
self.place_params = {}
self.initialize_pap_pick_place_params(moved_obj, parent_state)
self.nocollision_pick_op = {}
self.collision_pick_op = {}
self.determine_collision_and_collision_free_picks()
self.nocollision_place_op = {}
self.collision_place_op = {}
self.determine_collision_and_collision_free_places()
"""
print(
sum([o in self.collision_pick_op for o in objects]), sum([o in self.nocollision_pick_op for o in objects]),
sum([(o, r) in self.collision_place_op for o in objects for r in regions]),
sum([(o, r) in self.nocollision_place_op for o in objects for r in regions]))
"""
# predicates
self.pick_in_way = PickInWay(self.problem_env)
self.place_in_way = PlaceInWay(self.problem_env)
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()
utils.set_rightarm_torso(init_robot_config, self.problem_env.robot)
class MinimiumConstraintPaPState(State):
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 is_entity_reachable(self, entity):
return self.nodes[entity][-2]
def get_entities_in_place_way(self, entity, region):
inway = []
for obj_name in self.problem_env.object_names:
if self.ternary_edges[(entity, obj_name, region)][0]:
inway.append(obj_name)
return inway
def get_entities_in_pick_way(self, entity):
inway = []
for obj_name in self.problem_env.object_names:
if self.binary_edges[(obj_name, entity)][1]:
inway.append(obj_name)
return inway
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 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 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 update_cached_data_after_binary(self):
self.mc_pick_path = self.pick_in_way.mc_path_to_entity
if not self.use_shortest_path:
self.reachable_entities = self.pick_in_way.reachable_entities
self.pick_used = self.pick_in_way.pick_used
def update_cached_data_after_ternary(self):
self.place_used = self.place_in_way.place_used
self.mc_place_path = self.place_in_way.mc_path_to_entity
self.pick_used = self.place_in_way.pick_used
def get_binary_edges(self):
self.pick_in_way.set_pick_used(self.pick_used)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
key = (a, b)
if key not in edges.keys():
pick_edge_features = self.get_binary_edge_features(
a, b) # a = src, b = dest
edges[key] = pick_edge_features
self.update_cached_data_after_binary()
return edges
def get_ternary_edges(self):
self.place_in_way.set_pick_used(self.pick_used)
self.place_in_way.set_place_used(self.place_used)
self.place_in_way.set_reachable_entities(self.reachable_entities)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
for r in self.problem_env.regions:
key = (a, b, r)
if r.find('region') == -1 or r.find('entire') != -1:
continue
if key not in edges.keys():
place_edge_features = self.get_ternary_edge_features(
a, b, r)
edges[key] = place_edge_features
self.update_cached_data_after_ternary()
return edges
def get_nodes(self):
nodes = {}
for entity in self.problem_env.entity_names:
nodes[entity] = self.get_node_features(entity, self.goal_entities)
return nodes
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(),
]
def get_ternary_edge_features(self, a, b, r):
if (a, b, r) in self.parent_ternary_predicates:
return self.parent_ternary_predicates[(a, b, r)]
else:
key = (a, r)
if key in self.cached_place_paths:
# perhaps it is here that we set the mc path?
cached_path = self.cached_place_paths[key]
else:
cached_path = None
return [self.place_in_way(a, b, r, cached_path=cached_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 make_pklable(self):
self.problem_env = None
# self.is_reachable.problem_env = None
self.in_region.problem_env = None
self.pick_in_way.problem_env = None
self.pick_in_way.robot = None
self.is_holding_goal_entity.problem_env = None
self.place_in_way.problem_env = None
self.place_in_way.robot = None
for operator in self.pick_used.values():
operator.make_pklable()
for operator in self.place_used.values():
operator.make_pklable()
if self.parent_state is not None:
self.parent_state.make_pklable()
def make_plannable(self, problem_env):
self.problem_env = problem_env
# self.is_reachable.problem_env = problem_env
self.in_region.problem_env = problem_env
self.pick_in_way.problem_env = problem_env
self.pick_in_way.robot = problem_env.robot
self.place_in_way.problem_env = problem_env
self.place_in_way.robot = problem_env.robot
self.is_holding_goal_entity.problem_env = problem_env
if self.parent_state is not None:
self.parent_state.make_plannable(problem_env)
def print_chosen_entity(self, op):
is_goal_idx = -3
is_reachable_idx = -2
discrete_param = op.discrete_parameters['object']
discrete_param_node = self.nodes[discrete_param]
# Node info
is_reachable = discrete_param_node[is_reachable_idx]
is_goal_entity = discrete_param_node[is_goal_idx]
is_in_goal_region = self.binary_edges[(discrete_param,
'home_region')][1]
literal = "reachable %r goal %r in_goal_region %r" % (
is_reachable, is_goal_entity, is_in_goal_region)
print "Node literal", literal
# Edge info
goal_obj = 'rectangular_packing_box2'
goal_region = 'home_region'
pick_in_way_idx = 1
place_in_way_idx = 0
is_selecte_in_way_of_pick_to_goal_obj = self.binary_edges[(
discrete_param, goal_obj)][pick_in_way_idx]
is_selected_in_way_of_placing_goal_obj_to_goal_region = \
self.ternary_edges[(goal_obj, discrete_param, goal_region)][place_in_way_idx]
is_pick_in_way_to_goal_occluding_entity = False
is_place_in_way_to_goal_occluding_entity = False
goal_pick_occluding_entities = []
goal_place_occluding_entities = []
for obj in self.problem_env.objects:
obj_name = obj.GetName()
is_goal_obj_occluding_entity = self.binary_edges[(
obj_name, goal_obj)][pick_in_way_idx]
is_goal_region_occluding_entity = self.ternary_edges[(
goal_obj, obj_name, goal_region)][place_in_way_idx]
if is_goal_obj_occluding_entity:
goal_pick_occluding_entities.append(obj_name)
if is_goal_region_occluding_entity:
goal_place_occluding_entities.append(obj_name)
if is_goal_obj_occluding_entity or is_goal_region_occluding_entity:
# is the selected obj pick-in-way to the goal-occluding object?
if not is_pick_in_way_to_goal_occluding_entity:
is_pick_in_way_to_goal_occluding_entity = self.binary_edges[
(discrete_param, obj_name)][pick_in_way_idx]
if not is_place_in_way_to_goal_occluding_entity:
is_place_in_way_to_goal_occluding_entity = \
self.ternary_edges[(obj_name, discrete_param, 'loading_region')][place_in_way_idx]
print "Is goal-occluding?"
literal = "is_selecte_in_way_of_pick_to_goal_obj %r is_selected_in_way_of_placing_goal_obj_to_goal_region %r" \
% (is_selecte_in_way_of_pick_to_goal_obj, is_selected_in_way_of_placing_goal_obj_to_goal_region)
print literal
print "Is occluding a goal-occluding?"
literal = "is_blocking_pick_path_of_goal_occluding_entity %r is_blocking_place_path_of_goal_occluding_entity %r" \
% (is_pick_in_way_to_goal_occluding_entity, is_place_in_way_to_goal_occluding_entity)
print literal
print "goal_pick_occluding_objects", goal_pick_occluding_entities
print "goal_place_occluding_objects", goal_place_occluding_entities
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 __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()
class ShortestPathPaPState(PaPState):
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 make_key_config_obstacles_from_prm_collisions(self):
# make key configs
n_vtxs = len(self.prm_vertices)
collision_vector = np.zeros((n_vtxs))
colliding_vtx_idxs = [v for v in self.collides.values()]
colliding_vtx_idxs = list(set().union(*colliding_vtx_idxs))
collision_vector[colliding_vtx_idxs] = 1
collision_vector = np.delete(collision_vector,
[415, 586, 615, 618, 619],
axis=0)
return collision_vector
def initialize_parent_predicates(self, moved_obj, parent_state,
parent_action):
assert parent_action is not None
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
}
def get_pick_poses(self, object, moved_obj, parent_state):
if parent_state is not None and moved_obj != object.GetName():
return parent_state.pick_used[object.GetName()]
operator_skeleton = Operator('two_arm_pick', {'object': object})
generator = UniformGenerator(operator_skeleton, self.problem_env, None)
# we should disable objects, because we are getting shortest path that ignors all collisions anyways
self.problem_env.disable_objects_in_region('entire_region')
motion_plan_goals = []
n_iters = range(10, 500, 10)
for n_iter_to_try in n_iters:
op_cont_params, _ = generator.sample_feasible_op_parameters(
operator_skeleton,
n_iter=n_iter_to_try,
n_parameters_to_try_motion_planning=5)
motion_plan_goals = [
op['q_goal'] for op in op_cont_params
if op['q_goal'] is not None
]
if len(motion_plan_goals) > 2:
break
self.problem_env.enable_objects_in_region('entire_region')
# assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable
operator_skeleton.continuous_parameters[
'q_goal'] = motion_plan_goals # to make it consistent with Dpl
return operator_skeleton
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 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 get_binary_edges(self):
self.pick_in_way.set_pick_used(self.pick_used)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
key = (a, b)
if key not in edges.keys():
pick_edge_features = self.get_binary_edge_features(
a, b) # a = src, b = dest
edges[key] = pick_edge_features
return edges
def get_ternary_edges(self):
self.place_in_way.set_pick_used(self.pick_used)
self.place_in_way.set_place_used(self.place_used)
self.place_in_way.set_reachable_entities(self.reachable_entities)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
for r in self.problem_env.regions:
key = (a, b, r)
if r.find('region') == -1 or r.find('entire') != -1:
continue
if key not in edges.keys():
place_edge_features = self.get_ternary_edge_features(
a, b, r)
edges[key] = place_edge_features
return edges
def get_nodes(self):
nodes = {}
for entity in self.problem_env.entity_names:
nodes[entity] = self.get_node_features(entity, self.goal_entities)
return nodes
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:
is_entity_reachable = entity 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(),
]
def get_ternary_edge_features(self, a, b, r):
if (a, b, r) in self.parent_ternary_predicates:
return self.parent_ternary_predicates[(a, b, r)]
else:
key = (a, r)
if key in self.cached_place_paths:
cached_path = self.cached_place_paths[key]
else:
assert a.find('region') != -1
cached_path = None
if key in self.reachable_regions_while_holding:
# if reachable then nothing is in the way
is_b_in_way_of_reaching_r_while_holding_a = False
else:
is_b_in_way_of_reaching_r_while_holding_a = self.place_in_way(
a, b, r, cached_path=cached_path)
return [is_b_in_way_of_reaching_r_while_holding_a]
def get_binary_edge_features(self, a, b):
is_place_in_b_reachable_while_holding_a = (
a, b) in self.reachable_regions_while_holding
if b.find('region') != -1:
cached_path = None
else:
cached_path = self.cached_pick_paths[b]
"""
if (a, b) in self.parent_binary_predicates:
# we can only do below if the robot configuration didn't change much
is_a_in_pick_path_of_b = self.parent_binary_predicates[(a, b)][1]
else:
is_a_in_pick_path_of_b = self.pick_in_way(a, b, cached_path=cached_path)
"""
is_a_in_pick_path_of_b = self.pick_in_way(a,
b,
cached_path=cached_path)
return [
self.in_region(a, b), is_a_in_pick_path_of_b,
is_place_in_b_reachable_while_holding_a
]
def __init__(self, problem_env, goal_entities, parent_state=None, parent_action=None, planner='greedy', paps_used_in_data=None):
PaPState.__init__(self, problem_env, goal_entities, parent_state=None, parent_action=None,
paps_used_in_data=paps_used_in_data)
self.parent_state = parent_state
self.parent_ternary_predicates = {}
self.parent_binary_predicates = {}
self.object_names = [str(obj.GetName()) for obj in problem_env.objects]
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 = []
if paps_used_in_data is not None:
self.pick_used = copy.deepcopy(paps_used_in_data[0])
for obj in problem_env.objects:
if obj.GetName() not in self.pick_used:
self.pick_used[obj.GetName()] = self.get_pick_poses(obj, moved_obj, parent_state)
self.place_used = copy.deepcopy(paps_used_in_data[1])
else:
self.pick_used = {
object.GetName(): self.get_pick_poses(object, moved_obj, parent_state) for object in problem_env.objects
}
self.place_used = {}
if self.use_prm:
if parent_state is None:
self.collisions_at_all_obj_pose_pairs, self.collisions_at_current_obj_pose_pairs = self.update_collisions_at_prm_vertices(None)
else:
self.collisions_at_all_obj_pose_pairs, self.collisions_at_current_obj_pose_pairs = self.update_collisions_at_prm_vertices(parent_state.collisions_at_all_obj_pose_pairs)
self.holding_collides = None
self.current_holding_collides = None
"""
test_col_1 = set()
for tmp in self.collisions_at_all_obj_pose_pairs.values():
test_col_1 = test_col_1.union(tmp)
test_col_2 = set()
for tmp in self.collisions_at_current_obj_pose_pairs.values():
test_col_2 = test_col_2.union(tmp)
if len(test_col_1) != len(test_col_2.intersection(test_col_1)):
import pdb;pdb.set_trace()
"""
# 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)
saver.Restore()
"""
else:
self.holding_collides = None
self.holding_current_collides = None
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.collisions_at_current_obj_pose_pairs,
pick_poses=self.pick_used,
use_shortest_path=True)
self.place_in_way = PlaceInWay(self.problem_env,
collides=self.collisions_at_current_obj_pose_pairs,
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()
# note: the ternary and binary edges must be computed in this particular order
self.ternary_edges = self.get_ternary_edges()
self.binary_edges = self.get_binary_edges()
class ShortestPathPaPState(PaPState):
def __init__(self, problem_env, goal_entities, parent_state=None, parent_action=None, planner='greedy', paps_used_in_data=None):
PaPState.__init__(self, problem_env, goal_entities, parent_state=None, parent_action=None,
paps_used_in_data=paps_used_in_data)
self.parent_state = parent_state
self.parent_ternary_predicates = {}
self.parent_binary_predicates = {}
self.object_names = [str(obj.GetName()) for obj in problem_env.objects]
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 = []
if paps_used_in_data is not None:
self.pick_used = copy.deepcopy(paps_used_in_data[0])
for obj in problem_env.objects:
if obj.GetName() not in self.pick_used:
self.pick_used[obj.GetName()] = self.get_pick_poses(obj, moved_obj, parent_state)
self.place_used = copy.deepcopy(paps_used_in_data[1])
else:
self.pick_used = {
object.GetName(): self.get_pick_poses(object, moved_obj, parent_state) for object in problem_env.objects
}
self.place_used = {}
if self.use_prm:
if parent_state is None:
self.collisions_at_all_obj_pose_pairs, self.collisions_at_current_obj_pose_pairs = self.update_collisions_at_prm_vertices(None)
else:
self.collisions_at_all_obj_pose_pairs, self.collisions_at_current_obj_pose_pairs = self.update_collisions_at_prm_vertices(parent_state.collisions_at_all_obj_pose_pairs)
self.holding_collides = None
self.current_holding_collides = None
"""
test_col_1 = set()
for tmp in self.collisions_at_all_obj_pose_pairs.values():
test_col_1 = test_col_1.union(tmp)
test_col_2 = set()
for tmp in self.collisions_at_current_obj_pose_pairs.values():
test_col_2 = test_col_2.union(tmp)
if len(test_col_1) != len(test_col_2.intersection(test_col_1)):
import pdb;pdb.set_trace()
"""
# 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)
saver.Restore()
"""
else:
self.holding_collides = None
self.holding_current_collides = None
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.collisions_at_current_obj_pose_pairs,
pick_poses=self.pick_used,
use_shortest_path=True)
self.place_in_way = PlaceInWay(self.problem_env,
collides=self.collisions_at_current_obj_pose_pairs,
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()
# note: the ternary and binary edges must be computed in this particular order
self.ternary_edges = self.get_ternary_edges()
self.binary_edges = self.get_binary_edges()
def initialize_parent_predicates(self, moved_obj, parent_state, parent_action):
assert parent_action is not None
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
}
def get_pick_poses(self, object, moved_obj, parent_state):
if parent_state is not None and moved_obj != object.GetName():
return parent_state.pick_used[object.GetName()]
operator_skeleton = Operator('two_arm_pick', {'object': object})
generator = UniformPaPGenerator(None,
operator_skeleton,
self.problem_env,
None,
n_candidate_params_to_smpl=3,
total_number_of_feasibility_checks=500,
dont_check_motion_existence=True)
# we should disable objects, because we are getting shortest path that ignors all collisions anyways
self.problem_env.disable_objects_in_region('entire_region')
op_cont_params, _ = generator.sample_candidate_params_with_increasing_iteration_limit()
motion_plan_goals = [op['q_goal'] for op in op_cont_params if op['q_goal'] is not None]
"""
for n_iter_to_try in n_iters:
op_cont_params, _ = generator.sample_feasible_op_parameters(operator_skeleton,
n_iter=n_iter_to_try,
n_parameters_to_try_motion_planning=5)
# I see. So here, return no op['q_goal'] when it is not feasible.
motion_plan_goals = [op['q_goal'] for op in op_cont_params if op['q_goal'] is not None]
if len(motion_plan_goals) > 2:
break
"""
self.problem_env.enable_objects_in_region('entire_region')
assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable
operator_skeleton.continuous_parameters['q_goal'] = motion_plan_goals # to make it consistent with Dpl
if len(motion_plan_goals) == 0:
import pdb;pdb.set_trace()
return operator_skeleton
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']
try:
assert len(motion_plan_goals) > 0
except:
import pdb;pdb.set_trace()
self.cached_pick_paths[obj] = None
path, status = motion_planner.get_motion_plan(motion_plan_goals,
cached_collisions=self.collisions_at_all_obj_pose_pairs)
if status == 'HasSolution':
self.reachable_entities.append(obj)
else:
path, _ = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions={})
try:
assert path is not None
except:
import pdb;pdb.set_trace()
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 get_binary_edges(self):
self.pick_in_way.set_pick_used(self.pick_used)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
key = (a, b)
if key not in edges.keys():
pick_edge_features = self.get_binary_edge_features(a, b) # a = src, b = dest
edges[key] = pick_edge_features
return edges
def get_ternary_edges(self):
self.place_in_way.set_pick_used(self.pick_used)
self.place_in_way.set_place_used(self.place_used)
self.place_in_way.set_reachable_entities(self.reachable_entities)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
#for r in self.problem_env.regions:
for r in self.problem_env.entity_names:
key = (a, b, r)
is_r_not_region = r.find('region') == -1
if r.find('entire') != -1:
edges[key] = [False]
continue
if is_r_not_region or a == b:
edges[key] = [False]
continue
if key not in edges.keys():
place_edge_features = self.get_ternary_edge_features(a, b, r)
edges[key] = place_edge_features
return edges
def get_nodes(self):
nodes = {}
for entity in self.problem_env.entity_names:
nodes[entity] = self.get_node_features(entity, self.goal_entities)
return nodes
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:
is_entity_reachable = entity 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(),
]
def get_ternary_edge_features(self, a, b, r):
if (a, b, r) in self.parent_ternary_predicates:
return self.parent_ternary_predicates[(a, b, r)]
else:
key = (a, r)
if key in self.cached_place_paths:
cached_path = self.cached_place_paths[key]
else:
a_is_region = a.find('region') != -1
try:
assert a_is_region
except:
import pdb;pdb.set_trace()
cached_path = None
"""
if key in self.reachable_regions_while_holding:
# if reachable then nothing is in the way
is_b_in_way_of_reaching_r_while_holding_a = False
else:
"""
is_b_in_way_of_reaching_r_while_holding_a = self.place_in_way(a, b, r, cached_path=cached_path)
return [is_b_in_way_of_reaching_r_while_holding_a]
def get_binary_edge_features(self, a, b):
is_place_in_b_reachable_while_holding_a = (a, b) in self.reachable_regions_while_holding
"""
objs_occluding_moving_a_to_b = [occluding for occluding in self.object_names
if self.ternary_edges[(a, occluding, b)][0]]
if b in self.object_names or b == 'entire_region':
is_place_in_b_reachable_while_holding_a = False
else:
is_place_in_b_reachable_while_holding_a = len(objs_occluding_moving_a_to_b) == 0
"""
if 'region' in b:
cached_path = None
else:
cached_path = self.cached_pick_paths[b]
"""
if (a, b) in self.parent_binary_predicates:
# we can only do below if the robot configuration didn't change much
is_a_in_pick_path_of_b = self.parent_binary_predicates[(a, b)][1]
else:
is_a_in_pick_path_of_b = self.pick_in_way(a, b, cached_path=cached_path)
"""
is_a_in_pick_path_of_b = self.pick_in_way(a, b, cached_path=cached_path)
return [
self.in_region(a, b),
is_a_in_pick_path_of_b,
is_place_in_b_reachable_while_holding_a
]
class ShortestPathPaPState(PaPState):
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)
# print self.problem_env.robot.GetDOFValues()
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['object'])
if moved_obj_type == str or moved_obj_type == unicode:
moved_obj = parent_action.discrete_parameters['object']
else:
moved_obj = parent_action.discrete_parameters['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
"""
# what is this code snippet do?
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.place_used = {}
self.cached_pick_paths = {}
self.cached_place_paths = {}
self.cached_place_start_and_prm_idxs = {}
# print self.problem_env.robot.GetDOFValues()
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 initialize_parent_predicates(self, moved_obj, parent_state, parent_action):
assert parent_action is not None
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
}
def sample_feasible_picks(self, n_iters, generator, operator_skeleton, given_pick_base_pose):
feasible_cont_params = []
for n_iter_to_try in n_iters:
op_cont_params, _ = generator.sample_feasible_op_parameters(operator_skeleton,
n_iter=n_iter_to_try,
n_parameters_to_try_motion_planning=1,
chosen_pick_base_pose=given_pick_base_pose)
# feasible_cont_params = [op for op in op_cont_params if op['q_goal'] is not None]
if op_cont_params[0]['q_goal'] is not None:
feasible_cont_params.append(op_cont_params[0])
if len(feasible_cont_params) > 2:
break
return feasible_cont_params
def get_pick_poses(self, object, moved_obj, parent_state):
if parent_state is not None and moved_obj != object.GetName():
return parent_state.pick_used[object.GetName()]
operator_skeleton = Operator('two_arm_pick', {'object': object})
generator = UniformGenerator(operator_skeleton, self.problem_env, None)
# This is just generating pick poses. It does not check motion feasibility
self.problem_env.disable_objects_in_region('entire_region')
object.Enable(True)
self.problem_env.set_robot_to_default_dof_values()
n_iters = range(10, 500, 10)
feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, None)
if len(feasible_cont_params) == 0 and moved_obj == object.GetName():
given_base_pose = utils.get_robot_xytheta()
feasible_cont_params = self.sample_feasible_picks(n_iters, generator, operator_skeleton, given_base_pose)
orig_xytheta = get_body_xytheta(self.problem_env.robot)
self.problem_env.enable_objects_in_region('entire_region')
min_n_collisions = np.inf
chosen_pick_param = None
for cont_param in feasible_cont_params:
n_collisions = self.problem_env.get_n_collisions_with_objects_at_given_base_conf(cont_param['q_goal'])
if min_n_collisions > n_collisions:
chosen_pick_param = cont_param
min_n_collisions = n_collisions
utils.set_robot_config(orig_xytheta)
# assert len(motion_plan_goals) > 0 # if we can't find a pick pose then the object should be treated as unreachable
operator_skeleton.continuous_parameters = chosen_pick_param
operator_skeleton.continuous_parameters['q_goal'] = [chosen_pick_param['q_goal']]
return operator_skeleton
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
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_edges(self):
self.pick_in_way.set_pick_used(self.pick_used)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
key = (a, b)
if key not in edges.keys():
pick_edge_features = self.get_binary_edge_features(a, b) # a = src, b = dest
edges[key] = pick_edge_features
return edges
def get_ternary_edges(self):
self.place_in_way.set_pick_used(self.pick_used)
self.place_in_way.set_place_used(self.place_used)
self.place_in_way.set_reachable_entities(self.reachable_entities)
edges = {}
for a in self.problem_env.entity_names:
for b in self.problem_env.entity_names:
for r in self.problem_env.regions:
key = (a, b, r)
if r.find('region') == -1 or r.find('entire') != -1:
continue
if key not in edges.keys():
place_edge_features = self.get_ternary_edge_features(a, b, r)
edges[key] = place_edge_features
return edges
def get_nodes(self):
nodes = {}
for entity in self.problem_env.entity_names:
nodes[entity] = self.get_node_features(entity, self.goal_entities)
return nodes
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:
is_entity_reachable = entity 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(),
]
def get_ternary_edge_features(self, a, b, r):
if (a, b, r) in self.parent_ternary_predicates:
return self.parent_ternary_predicates[(a, b, r)]
else:
key = (a, r)
if key in self.cached_place_paths:
cached_path = self.cached_place_paths[key]
else:
assert a.find('region') != -1
cached_path = None
if key in self.reachable_regions_while_holding:
# if reachable then nothing is in the way
is_b_in_way_of_reaching_r_while_holding_a = False
else:
is_b_in_way_of_reaching_r_while_holding_a = self.place_in_way(a, b, r, cached_path=cached_path)
return [is_b_in_way_of_reaching_r_while_holding_a]
def get_binary_edge_features(self, a, b):
is_place_in_b_reachable_while_holding_a = (a, b) in self.reachable_regions_while_holding
if b.find('region') != -1:
cached_path = None
else:
cached_path = self.cached_pick_paths[b]
"""
if (a, b) in self.parent_binary_predicates:
# we can only do below if the robot configuration didn't change much
is_a_in_pick_path_of_b = self.parent_binary_predicates[(a, b)][1]
else:
is_a_in_pick_path_of_b = self.pick_in_way(a, b, cached_path=cached_path)
"""
is_a_in_pick_path_of_b = self.pick_in_way(a, b, cached_path=cached_path)
return [
self.in_region(a, b),
is_a_in_pick_path_of_b,
is_place_in_b_reachable_while_holding_a
]