def set_cached_pick_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') # making it consistent with the feasibility checker self.problem_env.set_robot_to_default_dof_values() for obj, op_instance in self.pick_used.items(): # print self.problem_env.robot.GetDOFValues() motion_plan_goals = op_instance.continuous_parameters['q_goal'] assert len(motion_plan_goals) > 0 self.cached_pick_paths[obj] = None path, status = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions=self.collides) if status == 'HasSolution': self.reachable_entities.append(obj) else: path, _ = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions={}) [t.Enable(False) for t in self.problem_env.objects] rrt_motion_planner = BaseMotionPlanner(self.problem_env, 'rrt') # motion_plan_goals[0] = np.array([-0.11255534, -0.26290062, 1.64126379]) # utils.set_robot_config(motion_plan_goals[0]) for _ in range(100): path, status = rrt_motion_planner.get_motion_plan(motion_plan_goals[0]) if status == 'HasSolution': break [t.Enable(True) for t in self.problem_env.objects] assert path is not None, 'Even RRT failed!' self.cached_pick_paths[obj] = path op_instance.low_level_motion = path
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()): path = [get_body_xytheta(self.problem_env.robot).squeeze()] #self.reachable_regions_while_holding.append((obj, region_name)) else: if self.holding_collides is not None: path, status = motion_planner.get_motion_plan(region, cached_collisions=self.holding_collides) else: # I think the trouble here is that we do not hold the object when checking collisions # So, the best way to solve this problem is to not keep reachable_regions_while_holding # and just use the cached path. But I am wondering how we got a colliding-path in # the first place. It must be from place_in_way? No, we execute this function first, # and then using the cached paths, compute the place_in_way. # Also, there is something wrong with the collision checking too. # I think this has to do with the fact that the collisions are computed using # the robot only, not with the object in hand. # So, here is what I propose: # Plan motions here, but do not add to reachable regions while holding. # This way, it will plan motions as if it is not holding the object, # but check collisions inside place_in_way path, status = motion_planner.get_motion_plan(region, cached_collisions=self.collisions_at_all_obj_pose_pairs) if status == 'HasSolution': pass else: obj_not_moved = obj != moved_obj parent_state_has_cached_path_for_obj = parent_state is not None \ and obj in parent_state.cached_place_paths # todo: What is this logic here...? # it is about re-using the parent place path; # but this assumes that the object has not moved? if parent_state_has_cached_path_for_obj and obj_not_moved: path = parent_state.cached_place_paths[(obj, region_name)] else: path, _ = motion_planner.get_motion_plan(region, cached_collisions={}) saver.Restore() # assert path is not None self.cached_place_paths[(obj, region_name)] = path
def set_cached_pick_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for obj, op_instance in self.pick_used.items(): motion_plan_goals = op_instance.continuous_parameters['q_goal'] self.cached_pick_paths[obj] = None if not self.use_shortest_path: continue if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj: self.cached_pick_paths[obj] = parent_state.cached_pick_paths[ obj] else: self.cached_pick_paths[obj] = motion_planner.get_motion_plan( motion_plan_goals, cached_collisions={})[0] if len(motion_plan_goals) == 0: # This pick path is used for checking the pickinway predicate and to pickup the object in place in way predicate. # assert False self.cached_pick_paths[obj] = None else: try: # how can this be, since we disable all the collisions? assert self.cached_pick_paths[obj] is not None except: import pdb pdb.set_trace() op_instance.continuous_parameters[ 'potential_q_goals'] = motion_plan_goals op_instance.continuous_parameters[ 'q_goal'] = self.cached_pick_paths[obj][-1]
def approximate_minimal_collision_path(self, goal_configuration, path_ignoring_all_objects, collisions_in_path_ignoring_all_objects, cached_collisions): enabled_objects = {obj.GetName() for obj in self.problem_env.objects} enabled_objects -= {obj.GetName() for obj in collisions_in_path_ignoring_all_objects} [o.Enable(False) for o in collisions_in_path_ignoring_all_objects] minimal_objects_in_way = [] minimal_collision_path = path_ignoring_all_objects for obj in collisions_in_path_ignoring_all_objects: obj.Enable(True) [o.Enable(False) for o in minimal_objects_in_way] enabled_objects.add(obj.GetName()) enabled_objects -= {obj.GetName() for obj in minimal_objects_in_way} if self.problem_env.name.find('one_arm') != -1: path, status = ArmBaseMotionPlanner.get_motion_plan(self, goal_configuration, cached_collisions=cached_collisions) else: path, status = BaseMotionPlanner.get_motion_plan(self, goal_configuration, cached_collisions=cached_collisions, n_iterations=[20, 50, 100]) if status != 'HasSolution': minimal_objects_in_way.append(obj) else: minimal_collision_path = path self.problem_env.enable_objects_in_region('entire_region') return minimal_collision_path
def check_obj_reachable(self, obj): if len(self.problem_env.robot.GetGrabbed()) > 0: return False obj = self.problem_env.env.GetKinBody(obj) obj_name = str(obj.GetName()) if self.problem_env.name.find('one_arm') != -1: op = Operator('one_arm_pick', {'object': obj}) else: op = Operator('two_arm_pick', {'object': obj}) if obj_name in self.pick_used: motion_plan_goals = self.pick_used[obj_name].continuous_parameters['q_goal'] else: motion_plan_goals = self.generate_potential_pick_configs(op, n_pick_configs=10) if motion_plan_goals is not None: motion_planner = BaseMotionPlanner(self.problem_env, 'prm') motion, status = motion_planner.get_motion_plan(motion_plan_goals) is_feasible_param = status == 'HasSolution' else: is_feasible_param = False if is_feasible_param: op.make_pklable() op.continuous_parameters = {'q_goal': motion[-1]} self.motion_plans[obj_name] = motion if obj_name not in self.pick_used: self.pick_used[obj_name] = op self.evaluations[obj_name] = True return True else: self.evaluations[obj_name] = False return False
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None parent_state_has_cached_path_for_obj \ = parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj cached_path_is_shortest_path = parent_state is not None and \ not (obj, region_name) in parent_state.reachable_regions_while_holding saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains(self.problem_env.env.GetKinBody(obj).ComputeAABB()): path = [get_body_xytheta(self.problem_env.robot).squeeze()] self.reachable_regions_while_holding.append((obj, region_name)) else: if region.name == 'home_region': # a location right at the entrance of home goal = [np.array([0.73064842, -2.85306871, 4.87927762])] else: goal = region if self.holding_collides is not None: path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions=self.holding_collides, return_start_set_and_path_idxs=True) else: # note: self.collides is computed without holding the object. path, status, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions=self.collides, return_start_set_and_path_idxs=True) if status == 'HasSolution': self.reachable_regions_while_holding.append((obj, region_name)) else: if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path: path = parent_state.cached_place_paths[(obj, region_name)] start_and_prm_idxs = parent_state.cached_place_start_and_prm_idxs[(obj, region_name)] else: path, _, start_and_prm_idxs = motion_planner.get_motion_plan(goal, cached_collisions={}, return_start_set_and_path_idxs=True) saver.Restore() # assert path is not None self.cached_place_paths[(obj, region_name)] = path self.cached_place_start_and_prm_idxs[(obj, region_name)] = start_and_prm_idxs
def set_cached_pick_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for obj, op_instance in self.pick_used.items(): motion_plan_goals = op_instance.continuous_parameters['q_goal'] assert len(motion_plan_goals) > 0 self.cached_pick_paths[obj] = None path, status = motion_planner.get_motion_plan( motion_plan_goals, cached_collisions=self.collides) if status == 'HasSolution': self.reachable_entities.append(obj) else: path, _ = motion_planner.get_motion_plan(motion_plan_goals, cached_collisions={}) assert path is not None self.cached_pick_paths[obj] = path op_instance.low_level_motion = path
def get_minimum_constraint_path_to(self, goal_config, target_obj): print "Planning to goal config:", goal_config if self.use_shortest_path: motion_planner = BaseMotionPlanner(self.problem_env, 'prm') else: motion_planner = MinimumConstraintPlanner(self.problem_env, target_obj, 'prm') motion, status = motion_planner.get_motion_plan(goal_config, cached_collisions=self.collides) if motion is None: return None, 'NoSolution' return motion, status
def two_arm_domain_region_reachability_check(self, region): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') print "Motion planning to ", region motion, status = motion_planner.get_motion_plan(self.problem_env.regions[region], cached_collisions=self.collides) if status == 'HasSolution': self.motion_plans[region] = motion self.evaluations[region] = True return True else: self.evaluations[region] = False return False
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None parent_state_has_cached_path_for_obj \ = parent_state is not None and obj in parent_state.cached_place_paths and obj != moved_obj cached_path_is_shortest_path = parent_state is not None and \ not (obj, region_name) in parent_state.reachable_regions_while_holding saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains( self.problem_env.env.GetKinBody(obj).ComputeAABB()): path = [get_body_xytheta(self.problem_env.robot).squeeze()] self.reachable_regions_while_holding.append( (obj, region_name)) else: if self.holding_collides is not None: path, status = motion_planner.get_motion_plan( region, cached_collisions=self.holding_collides) else: path, status = motion_planner.get_motion_plan( region, cached_collisions=self.collides) if status == 'HasSolution': self.reachable_regions_while_holding.append( (obj, region_name)) else: if parent_state_has_cached_path_for_obj and cached_path_is_shortest_path: path = parent_state.cached_place_paths[( obj, region_name)] else: path, _ = motion_planner.get_motion_plan( region, cached_collisions={}) saver.Restore() # assert path is not None self.cached_place_paths[(obj, region_name)] = path
def compute_path_ignoring_obstacles(self, goal_configuration): self.problem_env.disable_objects_in_region('entire_region') if self.target_object is not None: self.target_object.Enable(True) if self.problem_env.name.find('one_arm') != -1: path, status = ArmBaseMotionPlanner.get_motion_plan(self, goal_configuration) else: stime = time.time() path, status = BaseMotionPlanner.get_motion_plan(self, goal_configuration) print "Motion plan time", time.time()-stime self.problem_env.enable_objects_in_region('entire_region') if path is None: import pdb; pdb.set_trace() return path
def get_binary_edge_features(self, a, b): if (a, b) in self.parent_binary_predicates: return self.parent_binary_predicates[(a, b)] else: # todo rename cached_pick_path and cached_place_path as shortest paths if self.use_shortest_path: # todo this needs to be re-computed too when we are using shortest paths, because # this is true if for all b in (a,b,r), b is not in the way of shortest path to r while holidng a # Since the shortest path plans a path without collision-checking, this is not an accurate computation if a in self.problem_env.object_names and b in self.problem_env.region_names and b != 'entire_region': if a not in self.reachable_entities: is_place_in_b_reachable_while_holding_a = False else: saver = CustomStateSaver(self.problem_env.env) self.pick_used[a].execute( ) # it should not be in collision motion_planner = BaseMotionPlanner( self.problem_env, 'prm') # note that it doesn't check the collision with the object held plan, status = motion_planner.get_motion_plan( self.problem_env.regions[b], cached_collisions=self.collides) saver.Restore() is_place_in_b_reachable_while_holding_a = status == 'HasSolution' else: is_place_in_b_reachable_while_holding_a = False else: is_place_in_b_reachable_while_holding_a = ( a, b) in self.place_in_way.reachable_obj_region_pairs if self.use_shortest_path: if b.find('region') != -1: cached_path = None else: cached_path = self.cached_pick_paths[b] is_a_in_pick_path_of_b = self.pick_in_way( a, b, cached_path=cached_path) else: is_a_in_pick_path_of_b = self.pick_in_way(a, b) return [ self.in_region(a, b), is_a_in_pick_path_of_b, is_place_in_b_reachable_while_holding_a ]
def approximate_minimal_collision_path( self, goal_configuration, path_ignoring_all_objects, collisions_in_path_ignoring_all_objects, cached_collisions): # enabled objects = all the objects except the ones that are in collision enabled_objects = {obj.GetName() for obj in self.problem_env.objects} enabled_objects -= { obj.GetName() for obj in collisions_in_path_ignoring_all_objects } [o.Enable(False) for o in collisions_in_path_ignoring_all_objects] # enable object one by one. If we can find a path with it turned off, then it is not actually in the way. # minimal objects in way = a set of objects that we cannot ignore # for other objects in collisions_in_path_ignoring_all_objects, we can ignore minimal_objects_in_way = [] minimal_collision_path = path_ignoring_all_objects print "Approximating MCR path..." for obj in collisions_in_path_ignoring_all_objects: obj.Enable(True) [o.Enable(False) for o in minimal_objects_in_way] enabled_objects.add(obj.GetName()) enabled_objects -= { obj.GetName() for obj in minimal_objects_in_way } if self.problem_env.name.find('one_arm') != -1: path, status = ArmBaseMotionPlanner.get_motion_plan( self, goal_configuration, cached_collisions=cached_collisions) else: path, status = BaseMotionPlanner.get_motion_plan( self, goal_configuration, cached_collisions=cached_collisions, n_iterations=[20, 50, 100, 500, 1000]) if status != 'HasSolution': minimal_objects_in_way.append(obj) else: minimal_collision_path = path self.problem_env.enable_objects_in_region('entire_region') return minimal_collision_path
def set_cached_place_paths(self, parent_state, moved_obj): motion_planner = BaseMotionPlanner(self.problem_env, 'prm') for region_name, region in self.problem_env.regions.items(): if region.name == 'entire_region': continue for obj, pick_path in self.cached_pick_paths.items(): self.cached_place_paths[(obj, region_name)] = None if not self.use_shortest_path: continue if pick_path is None: continue if parent_state is not None and obj in parent_state.cached_pick_paths and obj != moved_obj: self.cached_place_paths[( obj, region_name)] = parent_state.cached_place_paths[( obj, region_name)] else: saver = CustomStateSaver(self.problem_env.env) pick_used = self.pick_used[obj] pick_used.execute() if region.contains( self.problem_env.env.GetKinBody( obj).ComputeAABB()): self.cached_place_paths[(obj, region_name)] = [ get_body_xytheta(self.problem_env.robot).squeeze() ] else: self.cached_place_paths[( obj, region_name)] = motion_planner.get_motion_plan( region, cached_collisions={})[0] if self.cached_place_paths[(obj, region_name)] is None: import pdb pdb.set_trace() saver.Restore() try: assert self.cached_place_paths[(obj, region_name)] is not None except: import pdb pdb.set_trace()
def get_node_features(self, entity, goal_entities): isobj = entity not in self.problem_env.regions obj = self.problem_env.env.GetKinBody(entity) if isobj else None pose = get_body_xytheta(obj)[0] if isobj else None if isobj: if self.use_shortest_path: motion_planner = BaseMotionPlanner(self.problem_env, 'prm') pick_for_obj = self.pick_used[obj.GetName()] plan, status = motion_planner.get_motion_plan( pick_for_obj.continuous_parameters['potential_q_goals'], cached_collisions=self.collides) pick_for_obj.low_level_motion = plan if status == 'HasSolution': pick_for_obj.continuous_parameters['q_goal'] = plan[-1] self.reachable_entities.append(entity) is_entity_reachable = True else: is_entity_reachable = False else: is_entity_reachable = obj.GetName() in self.reachable_entities else: is_entity_reachable = True return [ 0, # l 0, # w 0, # h pose[0] if isobj else 0, # x pose[1] if isobj else 0, # y pose[2] if isobj else 0, # theta entity not in self.problem_env.regions, # IsObj entity in self.problem_env.regions, # IsRoom entity in self.goal_entities, # IsGoal is_entity_reachable, self.is_holding_goal_entity(), ]