def __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None):
AbstractState.__init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None)
# predicates
self.is_reachable = None
self.in_way = None
self.in_region = InRegion(self.problem_env)
self.is_holding_goal_entity = IsHoldingGoalEntity(
self.problem_env, goal_entities)
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)
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()
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 __init__(self,
problem_env,
goal_entities,
parent_state=None,
parent_action=None):
AbstractState.__init__(problem_env, goal_entities, parent_state,
parent_action)
self.use_prm = problem_env.name.find('two_arm') != -1
if self.use_prm:
self.collides, self.current_collides = self.update_collisions_at_prm_vertices(
parent_state)
else:
self.collides = None
self.current_collides = None
# predicates
self.is_reachable = IsReachable(self.problem_env,
collides=self.current_collides)
self.in_way = InWay(self.problem_env, collides=self.current_collides)
self.in_region = InRegion(self.problem_env)
self.is_holding_goal_entity = IsHoldingGoalEntity(
self.problem_env, goal_entities)
# GNN todo write this in a separate function
self.goal_entities = goal_entities
if parent_state is not None:
self.nodes = {}
self.edges = {}
if parent_action.type.find('pick') != -1:
self.update_node_features(parent_state)
self.update_edge_features(parent_state)
elif parent_action.type.find('place') != -1:
assert len(self.problem_env.robot.GetGrabbed()) == 0
grand_parent_state = parent_state.parent
self.update_node_features(
grand_parent_state,
parent_action.discrete_parameters['object'])
self.update_edge_features(
grand_parent_state,
parent_action.discrete_parameters['object'])
else:
raise NotImplementedError
else:
self.nodes = self.get_nodes()
self.edges = self.get_edges()
self.update_reachability_based_on_inway()
# for debugging purpose; to be deleted later
reachable_idx = 9
self.reachable_entities = [
n for n in self.nodes if self.nodes[n][reachable_idx]
]
for entity in self.nodes:
if entity.find('region') != -1:
continue
if self.nodes[entity][reachable_idx]:
set_color(self.problem_env.env.GetKinBody(entity), [1, 1, 1])
else:
set_color(self.problem_env.env.GetKinBody(entity), [0, 0, 0])
self.print_reachable_entities()
self.print_inway_entities()
# if the goal object is not reachable with naive path, then declare infeasible problem
goal_object = self.goal_entities[0]
objects_with_mc_path = self.in_way.minimum_constraint_path_to_entity.keys(
)
is_pick_state = len(self.problem_env.robot.GetGrabbed()) == 0
if is_pick_state and not (goal_object
in self.is_reachable.reachable_entities
or goal_object in objects_with_mc_path):
print "Infeasible problem instance"