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 ]