Python Scenario_Generator.apply_impulse_and_run方法代码示例

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
    def find_qualitative_path_ptlike(self, action, zones, initial_zone):
        scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
        scenario.apply_impulse_and_run(action)
        traj = scenario.find_man_traj()
        b2contacts = scenario.find_contacts_with_mobile_objs()
        #print contacts
        pre_zone = initial_zone        
        path = [initial_zone]

        for traj_pt in traj:
            occupied_zone = -1
            man_position = Point(traj_pt)
            for i in xrange(len(zones)):
                if zones[i].contains(man_position):
                    occupied_zone = i
                    break
            # if out of scope, still wait to see if it will come back, quite slow
            if occupied_zone == -1 or occupied_zone == pre_zone:
                continue

            '''

            if occupied_zone == -1:
                # reach end zone
                path.append(-1)
                break

            elif occupied_zone == pre_zone:
                continue
            '''
            path.append(occupied_zone)
            pre_zone = occupied_zone

        return path, b2contacts

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
    def explore_qualitative_path(self, qualitative_path, direction_range):
        print "explore path: ", qualitative_path

        impulse_range = (IMPULSE_RANGE_X, direction_range)
        num_sampled_points = 100
        actions = self.__sample_n_points_from_range(num_sampled_points, impulse_range)
        for action in actions:
            #path, contacts_info =  self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
            path = self.find_qualitative_path_rich(action, self.zones, self.initial_zone)
            print "sampled path: ", path
            #path = str(self.make_path_complete(path, self.graph)).strip('[]')
            if qualitative_path not in path:
                continue
            else:
                #print "perturb_action: ", action
                max_mu = self.perturb_action(action, qualitative_path)
                mu_list = np.random.normal(max_mu, 20, 100)                

                for mu in mu_list:
                    _action = (mu, action[1])
                    scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
                    scenario.apply_impulse_and_run(_action)
                    self.simulation_counter = self.simulation_counter + 1
                    print "sample action: ", _action               
                    if scenario.solved:
                        print "solution detected ", _action, " ", self.simulation_counter
                        return _action
        return -1

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
 def __apply__action__(self, impulse):
     scenario = Scenario_Generator(
         self.width,
         self.height,
         self.immobile_objs,
         self.mobile_objs,
         self.manipulatable_obj,
         self.target_obj,
         gravity_dir=self.gravity_dir,
         showRender=True,
     )
     scenario.apply_impulse_and_run(impulse)

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
 def explore_path(self, sampled_actions, root_path):
     print "explore path: ", root_path
     num_sampled_points = 100
     solution = -1
     for action in sampled_actions:
         max_mu = self.perturb_action(action, root_path)
         mu_list = np.random.normal(max_mu, 20, 100)
         for mu in mu_list:
             _action = (mu, action[1])
             scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
             scenario.apply_impulse_and_run(_action)
             self.simulation_counter = self.simulation_counter + 1
             if scenario.solved:
                 print "solved !!!!!!!!!!!!!!: ", self.simulation_counter
                 return _action
     return solution

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
    def explore_qualitative_path(self, qualitative_path, direction_range, essential_contact):
        print "explore path: ", qualitative_path, " essential_contact with: ", essential_contact, " direction range: ", direction_range

        impulse_range = (IMPULSE_RANGE_X, direction_range)
        num_sampled_points = 100
        actions = self.__sample_n_points_from_range(num_sampled_points, impulse_range)
        for action in actions:
            path, contacts_info =  self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
            #print contacts_info
            #print action, path, " essential_contact: ", essential_contact, " : ", contacts_info
            essential_contact_detected = False
            for contact in contacts_info:
                if contact[0] == essential_contact:
                    essential_contact_detected = True
                if contact[0] == self.target_obj_id:
                        print "solution detected ", action, " ", self.simulation_counter, str(self.make_path_complete(path, self.graph)).strip('[]')


            if not essential_contact_detected:
                continue
            #print " path before:  ", path
            path = str(self.make_path_complete(path, self.graph)).strip('[]')
            #print "path after: " ,path
           
            print "perturb_action: ", action
            max_mu = self.perturb_action(action, path, essential_contact)
            mu_list = np.random.normal(max_mu, 20, 100)                

            for mu in mu_list:
                _action = (mu, action[1])
                scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
                scenario.apply_impulse_and_run(_action)
                self.simulation_counter = self.simulation_counter + 1
                #print "sample action: ", _action               
                if scenario.solved:
                    print "solution detected ", _action, " ", self.simulation_counter
                    return _action
        return -1

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]

#.........这里部分代码省略.........
            if comp_path not in quali_paths:
                quali_paths.append(comp_path)
                
                    
            if path_dir not in path_by_dir:                        
                path_by_dir[path_dir] = [comp_path]
                if len(bounce_pos_list) > 0:
                    path_bounces[path_dir] = [bounce_pos_list]
                    path_first_bounces[path_dir] = set([bounce_pos_list[0]])
                else:
                    path_first_bounces[path_dir] = set([])
                    path_bounces[path_dir] = []
            else:
                path_by_dir[path_dir].append(comp_path)
                if len(bounce_pos_list) > 0:
                    path_bounces[path_dir].append(bounce_pos_list)
                    path_first_bounces[path_dir].add(bounce_pos_list[0])

        sort_dirs = []
        for path_dir in path_bounces:
            # calculate average bounce distance
            bounce_pos_list = path_bounces[path_dir]
            average_distance = 0
            for bounce_pos in bounce_pos_list:
                total_distance = 0
                r = 0.6
                for i in xrange(len(bounce_pos) - 1):
                    #print " ",  " bounce1:", bounce_pos[i], "  bounce2: ", bounce_pos[i+1],  self.zone_distance[(bounce_pos[i], bounce_pos[i+1])], 
                    #total_distance = total_distance + self.zone_distance[(bounce_pos[i], bounce_pos[i+1])]
                    #total_distance = total_distance + self.zone_distance[(bounce_pos[i], bounce_pos[i+1])] * pow(r,len(bounce_pos) - i)
                    total_distance = total_distance + self.zone_distance[(bounce_pos[i], bounce_pos[i+1])] * pow(1+r, i)
                #average_distance = average_distance + total_distance/len(bounce_pos)
                average_distance = average_distance + total_distance
            if len(bounce_pos_list) == 0:
                average_distance = 0
            else: 
                average_distance = average_distance/len(bounce_pos_list)
            print "path_dir: ", path_dir, "  ", average_distance

            heappush(sort_dirs, (average_distance, path_dir))
        '''
        while all_paths:
            path, bounces_pos = heappop(all_paths)
            print path, " bounces: ", bounces_pos
        '''
        while sort_dirs:
        #for path_dir in path_by_dir:
            distance, path_dir = heappop(sort_dirs)
            print "Test dir range: ", path_dir, " distance ", distance 
            #bounces_count = 0

            #for bounces_pos in path_bounces[path_dir]:
            #    print bounces_pos
            # subdivide path_dir into 10 sectors
            #quali_paths = path_by_dir[path_dir]
            divided_sectors = self.divide_dir(path_dir)
            use_less_path = False
            num_iter = 10 #4
            while num_iter > 0 and not use_less_path:
                num_iter = num_iter - 1
                bounces_count = 0
                for sector in divided_sectors:
                    num_samples = 10
                    impulse_range = (IMPULSE_RANGE_X, sector)
                    actions = self.__sample_n_points_from_range(num_samples, impulse_range)
                    #print "test sector: ", sector
                    #print actions
                    for action in actions:

                        path, contacts_info, solved = self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
                        if solved:
                            print "solution detected: ", action, "  ", self.simulation_counter
                            continue

                        essential_contact_detected = False
                        path = self.make_path_complete(path, self.graph)
                        for first_bounces in path_first_bounces[path_dir]:
                            if first_bounces in path:
                                bounces_count = bounces_count + 1

                        for contact in contacts_info:
                           if contact[0] in essential_contacts:                               
                                path_str = str(self.make_path_complete(path, self.graph)).strip('[]')
                                #print "path after: " ,path
                                print "perturb_action: ", action
                                max_mu = self.perturb_action(action, path_str, essential_contact)
                                mu_list = np.random.normal(max_mu, 20, 100)                

                                for mu in mu_list:
                                    _action = (mu, action[1])
                                    scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
                                    scenario.apply_impulse_and_run(_action)
                                    self.simulation_counter = self.simulation_counter + 1
                                    #print "sample action: ", _action               
                                    if scenario.solved:
                                        print "solution detected ", _action, " ", self.simulation_counter
        
                if bounces_count == 0:
                    print "exit at: ", 10 - num_iter
                    use_less_path = True

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
    def solve_with_rules_classify(self):
        all_paths = []
        classification = {}
        essential_contacts = set([])
        quali_paths = []
        sectors_score = []
        for qualitative_paths_group in self.estimated_qualitative_paths_groups:
            path_by_dir = {}
            processed_paths = []
            direction, estimated_qualitative_paths = qualitative_paths_group

            for path_dir, path, essential_contact in estimated_qualitative_paths:
                comp_path = self.make_path_complete(path, self.graph)
                if comp_path not in quali_paths:
                    quali_paths.append(comp_path)
                essential_contacts.add(essential_contact)
                if (path_dir[0], comp_path) not in processed_paths:
                    processed_paths.append(comp_path)
                    if path_dir[0] not in path_by_dir:                        
                        path_by_dir[path_dir[0]] = [comp_path]
                    else:
                        path_by_dir[path_dir[0]].append(comp_path)

            for path_dir in path_by_dir:
                # subdivide path_dir into 10 sectors
                #quali_paths = path_by_dir[path_dir]
                divided_sectors = self.divide_dir(path_dir)
                
                for sector in divided_sectors:
                    total_distance = 0
                    min_distance = 999
                    num_samples = 20
                    impulse_range = (IMPULSE_RANGE_X, sector)
                    actions = self.__sample_n_points_from_range(num_samples, impulse_range)
                    for action in actions:
                        path, contacts_info = self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
                        essential_contact_detected = False
                        path = self.make_path_complete(path, self.graph)
                        for contact in contacts_info:

                            if contact[0] in essential_contacts:                               
                                if contact[0] == self.target_obj_id:
                                    print "solution detected ", action, " ", self.simulation_counter, str(self.make_path_complete(path, self.graph)).strip('[]')
                                else:
                                    path_str = str(self.make_path_complete(path, self.graph)).strip('[]')
                                    #print "path after: " ,path
                                    print "perturb_action: ", action
                                    max_mu = self.perturb_action(action, path_str, essential_contact)
                                    mu_list = np.random.normal(max_mu, 20, 100)                

                                    for mu in mu_list:
                                        _action = (mu, action[1])
                                        scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
                                        scenario.apply_impulse_and_run(_action)
                                        self.simulation_counter = self.simulation_counter + 1
                                        #print "sample action: ", _action               
                                        if scenario.solved:
                                            print "solution detected ", _action, " ", self.simulation_counter
            

                        distance = self.least_distance(path, quali_paths)
                        if distance < min_distance:
                            min_distance = distance
                        total_distance = total_distance + distance

                    heappush(sectors_score, (total_distance, sector))

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
    def find_qualitative_path_rich(self, action, zones, initial_zone):
        scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
        scenario.apply_impulse_and_run(action)
        traj = scenario.find_man_traj()
        num_zones = len(zones)
        initial_time, x,y, touching = traj[0]
        # ball must touch the surface
        last_touching_time = initial_time # beginning of the last touching
        last_state_time = initial_time
        last_touching_state = True # False: not touched, True, touched
        pre_zone = initial_zone
        last_motion_type = 0
        quali_path = ''
        for traj_pt in traj:
            time, x, y, touching = traj_pt
            man_position = Point(x,y)

            for i in xrange(num_zones):
                if zones[i].contains(man_position):
                    occupied_zone = i
                    break

            if occupied_zone == pre_zone:
                if last_touching_state == touching:
                    continue
                else:
                    # previously touch, currently not
                    if last_touching_state: 
                        time_duration = time - last_touching_time
                        if time_duration > 30: #SLIDING
                            motion_type = 4
                           
                        else:
                            motion_type = 8 # BOUCING                           
                        
                    else:
                        motion_type = 2 # FLYING                    
                        last_touching_time = time

                    last_motion_type = last_motion_type | motion_type                    
                    last_motion_type = motion_type
                    last_touching_state = touching

            else:
                if time - last_state_time < 10:
                    continue
                if last_motion_type == 0:
                    if last_touching_state:
                        time_duration = time - last_touching_time
                        if time_duration > 30:
                            last_motion_type = 4
                        else:
                            last_motion_type = 8
                    else:
                        last_motion_type = 2


                quali_path = quali_path + str(pre_zone) + "*" +str(last_motion_type) + ","
                last_touching_state = touching
                pre_zone = occupied_zone
                last_state_time = time
                last_motion_type = 0
                if touching:
                    last_touching_time = time   


        return quali_path

# 需要导入模块: from scenario_generator import Scenario_Generator [as 别名]
# 或者: from scenario_generator.Scenario_Generator import apply_impulse_and_run [as 别名]
    def solve_with_rules_classify(self):
        all_paths = []
        classification = {}
        essential_contacts = set([])
        quali_paths = []
        sectors_score = []
        path_by_dir = {}
        path_bounces = {}
        for path_dir, path, essential_contact, bounces_pos in self.estimated_qualitative_paths:  
           # print bounces_pos
            heappush(all_paths, (len(bounces_pos), (path, bounces_pos)))
            #print path_dir


            comp_path = self.make_path_complete(path, self.graph)
            essential_contacts.add(essential_contact)
            if comp_path not in quali_paths:
                quali_paths.append(comp_path)
                
                    
            if path_dir not in path_by_dir:                        
                path_by_dir[path_dir] = [comp_path]
                path_bounces[path_dir] = [bounces_pos]
            else:
                path_by_dir[path_dir].append(comp_path)
                path_bounces[path_dir].append(bounces_pos)
        '''
        while all_paths:
            path, bounces_pos = heappop(all_paths)
            print path, " bounces: ", bounces_pos
        '''
        for path_dir in path_by_dir:

            print "Test dir range: ", path_dir
            for bounces_pos in path_bounces[path_dir]:
                print bounces_pos
            # subdivide path_dir into 10 sectors
            #quali_paths = path_by_dir[path_dir]
            divided_sectors = self.divide_dir(path_dir)
            
            for sector in divided_sectors:
                total_distance = 0
                min_distance = 999
                num_samples = 40
                impulse_range = (IMPULSE_RANGE_X, sector)
                actions = self.__sample_n_points_from_range(num_samples, impulse_range)
                #print "test sector: ", sector
                #print actions
                for action in actions:
                    path, contacts_info, solved = self.find_qualitative_path_ptlike(action, self.zones, self.initial_zone)
                    if solved:
                        print "solution detected: ", action, "  ", self.simulation_counter
                        continue

                    essential_contact_detected = False
                    path = self.make_path_complete(path, self.graph)
                   
                    for contact in contacts_info:

                        ############### ERROR, Target object will not be in contacts ############################
                        '''
                        if contact[0] == self.target_obj_id:
                                print "solution detected ", action, " ", self.simulation_counter, str(self.make_path_complete(path, self.graph)).strip('[]')
                        '''
                        #####################################################################################
                        if contact[0] in essential_contacts:                               
                            path_str = str(self.make_path_complete(path, self.graph)).strip('[]')
                            #print "path after: " ,path
                            print "perturb_action: ", action
                            max_mu = self.perturb_action(action, path_str, essential_contact)
                            mu_list = np.random.normal(max_mu, 20, 100)                

                            for mu in mu_list:
                                _action = (mu, action[1])
                                scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
                                scenario.apply_impulse_and_run(_action)
                                self.simulation_counter = self.simulation_counter + 1
                                #print "sample action: ", _action               
                                if scenario.solved:
                                    print "solution detected ", _action, " ", self.simulation_counter
        

                    distance = self.least_distance(path, quali_paths)
                    if distance < min_distance:
                        min_distance = distance
                    total_distance = total_distance + distance

                heappush(sectors_score, (total_distance, sector))