コード例 #1
0
ファイル: square_world.py プロジェクト: MengGuo/P_MAS_TG
def create_rectworld(Xs, Ys, eight_connected=False):
    '''
    create a rectangular world with square cells.
    eight_connected True if diagonal motion is allowed.
    '''
    node_dict = {}
    symbols = []
    X_range = range(0, Xs)
    Y_range = range(0, Ys)
    for X in X_range:
        for Y in Y_range:
            node_name = rectworld_nodename(X, Y)
            node_dict[(X,Y)] = set([node_name,])
            symbols.append(node_name)
    g = MotionFts(node_dict, symbols, 'rectworld')
    for X in X_range:
        for Y in Y_range:
            nodef = (X, Y)
            if eight_connected:
                offsets = product([-1,0,1],[-1,0,1])
            else:
                offsets = [(0,0),(0,1),(0,-1),(1,0),(-1,0)]
            for (dx,dy) in offsets:
                xt = X + dx
                yt = Y + dy
                if xt not in X_range or yt not in Y_range:
                    continue
                nodet = (xt, yt)
                unit_cost = 1
                g.add_edge(nodef, nodet, weight=unit_cost*(abs(dx)+abs(dy)))
    return g
コード例 #2
0
ファイル: square_world.py プロジェクト: elecshan/P_MAS_TG
def create_rectworld(Xs, Ys, eight_connected=False):
    '''
    create a rectangular world with square cells.
    eight_connected True if diagonal motion is allowed.
    '''
    node_dict = {}
    symbols = []
    X_range = range(0, Xs)
    Y_range = range(0, Ys)
    for X in X_range:
        for Y in Y_range:
            node_name = rectworld_nodename(X, Y)
            node_dict[(X, Y)] = set([
                node_name,
            ])
            symbols.append(node_name)
    g = MotionFts(node_dict, symbols, 'rectworld')
    for X in X_range:
        for Y in Y_range:
            nodef = (X, Y)
            if eight_connected:
                offsets = product([-1, 0, 1], [-1, 0, 1])
            else:
                offsets = [(0, 0), (0, 1), (0, -1), (1, 0), (-1, 0)]
            for (dx, dy) in offsets:
                xt = X + dx
                yt = Y + dy
                if xt not in X_range or yt not in Y_range:
                    continue
                nodet = (xt, yt)
                unit_cost = 1
                g.add_edge(nodef,
                           nodet,
                           weight=unit_cost * (abs(dx) + abs(dy)))
    return g
コード例 #3
0
    def _plan_syn(self, map_name, ap, regions, edges, action, hard_task):
        self.PlanViewer.clear()

        #self.s.login('12.0.5.2', 'ubuntu', 'ubuntu', original_prompt='$$S', port=22, auto_prompt_reset=True)

        if not (map_name and hard_task and self.InitPosInputR1.text()):
            if not map_name:
                self.PlanViewer.append('Please select a map first')
            elif not hard_task:
                self.PlanViewer.append('Please input a task first')
            else:
                self.PlanViewer.append('Please input a initial position')

        else:
            start_pos = regions.keys()[regions.values().index(
                set([str(self.InitPosInputR1.text())]))]
            # here the region must start with lower-case r instead of capital R

            robot_motion = MotionFts(regions, ap, map_name)
            robot_motion.set_initial(start_pos)
            robot_motion.add_un_edges(edges, unit_cost=0.1)
            robot_action = ActionModel(action)
            robot_model = MotActModel(robot_motion, robot_action)
            sys.stdout = EmittingStream(textWritten=self.text_written)
            robot_planner = ltl_planner(robot_model, hard_task, None)
            robot_planner.optimal(10, 'static')
            sys.stdout = sys.__stdout__

            self.PlanViewer.append('The start point is: ' + str(start_pos))
            self.PlanViewer.append('It will move to the next point: ' +
                                   str(robot_planner.next_move))

            stream = file(
                os.path.join(self.rp.get_path('rqt_ltl'), 'map', 'task.yaml'),
                'w')
            data = dict(total_task=hard_task + ',' + '',
                        start_point=start_pos,
                        map_name=map_name)
            yaml.dump(data,
                      stream,
                      default_flow_style=False,
                      allow_unicode=False)
コード例 #4
0
ファイル: office_sim.py プロジェクト: ptajvar/P_MAS_TG
              (33, -43, 5): set(['p7',]),
              (45, -45, 5): set(['p8',]),
              (60, -50, 5): set(['p9',]),
}

edges = [('p1', 'p2'),
         ('p2', 'p7'),
         ('p2', 'p4'),
         ('p4', 'p6'),
         ('p4', 'p8'),
         ('p6', 'p9'),
         ('p2', 'p3'),
         ('p3', 'p5'),         
]

robot_motion = MotionFts(regions, ap, 'office' )
robot_motion.set_initial((15, -45, 5))
robot_motion.add_un_edges_by_ap(edges, unit_cost = 0.1)


##############################
# action FTS
############# no action model
action = dict()
############# with action
robot_action = ActionModel(action)


##############################
# complete robot model
robot_model = MotActModel(robot_motion, robot_action)
コード例 #5
0
ファイル: agent_init.py プロジェクト: MengGuo/P_MAS_TG
# Model and task for Observing Youbot (OY)
#  |         oyr2
#  |oyr3
#  |
#  |oyr0     oyr1
################ OY motion  ##################
OY_node=[(0.01,0.01,0.0),(0.57,1.00,-1.57),(0.67,-0.38,-1.57),(-0.00,0.00,-2.6)]
OY_label={
    OY_node[0] : set(['oyr0']),
    OY_node[1]: set(['oyr1',]),
    OY_node[2]: set(['oyr2',]),
    OY_node[3]: set(['oyr3',]),
}
OY_init_pose=OY_node[0]
OY_symbols=set(['oyr0','oyr1','oyr2','oyr3'])
OY_motion=MotionFts(OY_label,OY_symbols,'OY-workspace')
OY_motion.set_initial(OY_init_pose)
OY_edge=[(OY_node[0],OY_node[1]),(OY_node[1],OY_node[2]),(OY_node[2],OY_node[3]),
(OY_node[1],OY_node[3]),]
OY_motion.add_un_edges(OY_edge,unit_cost=10.0)
########### OY action ##########
OY_action_label={
             'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])),
            }
OY_action = ActionModel(OY_action_label)
########### OY task ############
#OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)'
OY_task = '<>(oyr1 && <> (oyr2 && <> oyobsgrasp)) && ([]<> oyr1) && ([]<> oyr2)'
########### OY initialize ############
init['OY']=(OY_motion, OY_action, OY_task)
コード例 #6
0
ファイル: case.py プロジェクト: roussePaul/pwa
              (2, 0, 1): set(['r3', 'r']),
              (0, 1, 1): set(['r4', 'r']),
              (1, 1, 1): set(['r5', 'b']),
              (2, 1, 1): set(['r6', 'b']),
}

edges = [((0, 0, 1), (1, 0, 1)),
         ((1, 0, 1), (2, 0, 1)), 
         ((0, 1, 1), (1, 1, 1)),          
         ((1, 1, 1), (2, 1, 1)),
         ((0, 0, 1), (0, 1, 1)),
         ((1, 0, 1), (1, 1, 1)),
         ((2, 0, 1), (2, 1, 1)),          
]

robot_motion = MotionFts(regions, ap, 'office' )
robot_motion.set_initial((0, 0, 1))
robot_motion.add_un_edges(edges, unit_cost = 0.1)


##############################
# action FTS
############# no action model
#action = dict()
############# with action
action = { 'pick': (100, 'r', set(['pick'])),
           'drop': (50, 'b', set(['drop']))
}


robot_action = ActionModel(action)
コード例 #7
0
    (0, 1, 1): set(['r4', 'r']),
    (1, 1, 1): set(['r5', 'b']),
    (2, 1, 1): set(['r6', 'b']),
}

edges = [
    ((0, 0, 1), (1, 0, 1)),
    ((1, 0, 1), (2, 0, 1)),
    ((0, 1, 1), (1, 1, 1)),
    ((1, 1, 1), (2, 1, 1)),
    ((0, 0, 1), (0, 1, 1)),
    ((1, 0, 1), (1, 1, 1)),
    ((2, 0, 1), (2, 1, 1)),
]

robot_motion = MotionFts(regions, ap, 'office')
robot_motion.set_initial((0, 0, 1))
robot_motion.add_un_edges(edges, unit_cost=0.1)

##############################
# action FTS
############# no action model
#action = dict()
############# with action
action = {'pick': (100, 'r', set(['pick'])), 'drop': (50, 'b', set(['drop']))}

robot_action = ActionModel(action)

##############################
# complete robot model
robot_model = MotActModel(robot_motion, robot_action)
コード例 #8
0
# A1 fts
A1_init_pose = [2.0, 2.0, 0.0]
A1_node_dict = {
    (2.0, 2.0, 0.1): set(['r0']),
    (0.5, 0.5, 0.2): set(['r1', 'a']),
    (3.5, 0.5, 0.2): set(['r2']),
    (0.5, 2.0, 0.2): set(['r3']),
    (3.5, 2.0, 0.2): set(['r4', 'b']),
    (0.5, 3.3, 0.2): set(['r5']),
    (3.5, 3.3, 0.2): set(['r6']),
    (2.0, 1.0, 0.3): set(['r7']),
    (2.0, 3.0, 0.3): set(['r8']),
}
A1_symbols = set(
    ['r0', 'r1', 'r2', 'r3', 'r4', 'r5', 'r6', 'r7', 'r8', 'a', 'b'])
A1_motion = MotionFts(A1_node_dict, A1_symbols, 'A1_FTS')
A1_motion.set_initial(A1_init_pose)
A1_motion.add_full_edges(unit_cost=1.0 / SPEED[0])
# A1 action
A1_action_dict = {
    'la': (COST, 'a', set(['la'])),
    'ua': (COST, '1', set(['ua'])),
    'lb': (COST, 'b', set(['lb'])),
    'ub': (COST, '1', set(['ub'])),
}
dep['lb'] = set([
    'hb',
])
dep['ub'] = set([
    'hb',
])
コード例 #9
0
    ]),
    (-1, 6, 1): set([
        'r5',
    ]),
}

edges = [
    ((0, 0, 1), (3, 0, 1)),
    ((0, 0, 1), (3, 4, 1)),
    ((3, 0, 1), (3, 4, 1)),
    ((3, 0, 1), (3, 6, 1)),
    ((3, 4, 1), (3, 6, 1)),
    ((3, 6, 1), (-1, 6, 1)),
]

robot_motion = MotionFts(regions, ap, 'map1')
robot_motion.add_un_edges(edges, unit_cost=0.1)
action = dict()
robot_action = ActionModel(action)
init_pose = PoseWithCovarianceStamped()
current_pose = [init_pose.header.stamp, [0, 0, 1]]
hard_task = ''
soft_task = ''

# Predefined robot FTS model. It will be defined in a file or link to the GUI in the future


def task_callback(total_task):
    '''
    Subscriber callback of task_publisher. Parse task into hard and soft part
    :param total_task: hard_task + ',' + soft_task
コード例 #10
0
def compose_sys_fts(sys_models, add_data, symbols, buffer_size, com_rad=1):
    #----------------------------
    print 'compose_fts starts'
    t0 = time.time()
    #--------------------
    nodes = set()
    # construct nodes
    N = len(sys_models)
    N_f = int(floor(0.75 * N))
    ind_nodes = []
    for k in range(0, N):
        regs = sys_models[k][0].nodes()
        bufs = range(buffer_size[k] + 1)
        new_s = []
        for item in product(regs, bufs):
            # (reg,d)
            new_s.append(item)
        ind_nodes.append(new_s)
    #--------------------
    comp_nodes = dict()
    for items in product(*ind_nodes):
        prop = set()
        for k in range(0, N):
            fts = sys_models[k][0]
            reg = items[k][0]
            label = fts.node[reg]['label']
            prop.update(label)
        comp_nodes[tuple(items)] = prop
    comp_symbols = symbols
    # print comp_nodes
    #------------------------------
    comp_motion = MotionFts(comp_nodes, comp_symbols, 'comp_FTS')
    # build transitions
    for f_n in comp_nodes:
        # [(reg1,d1),(reg2,d2)...]
        for t_n in comp_nodes:
            if (f_n != t_n):
                allowed = True
                a_weight = 0
                for i in range(0, N):
                    f_n_i = f_n[i][0]
                    t_n_i = t_n[i][0]
                    fts_i = sys_models[i][0]
                    if (t_n_i in fts_i.successors(f_n_i)):
                        label = fts_i.edge[f_n_i][t_n_i]['label']
                        f_d_i = f_n[i][1]
                        t_d_i = t_n[i][1]
                        if (label != 'goto') and (label != 'ul'):
                            extra_data = add_data[label]
                            if (t_d_i == f_d_i + extra_data):
                                e_weight = fts_i.edge[f_n_i][t_n_i]['weight']
                                if e_weight > a_weight:
                                    a_weight = e_weight
                            else:
                                allowed = False
                                break
                                # data gather actions
                        elif (label == 'goto'):
                            one_ul = False
                            for j in range(N_f, N):
                                if t_n[j][0][1] == 'ul':
                                    one_ul = True
                                    break
                            if ((t_d_i == f_d_i)
                                    or ((t_d_i != f_d_i) and (t_d_i == 0) and
                                        (one_ul) and (i in range(0, N_f)))):
                                e_weight = fts_i.edge[f_n_i][t_n_i]['weight']
                                if e_weight > a_weight:
                                    a_weight = e_weight
                            else:
                                allowed = False
                                break
                        elif (label == 'ul'):
                            t_reg_i = t_n_i[0]
                            all_empty = True
                            for j in range(0, N_f):
                                t_reg_j = t_n[j][0][0]
                                t_d_j = t_n[j][1]
                                f_d_j = f_n[j][1]
                                if distance(t_reg_i, t_reg_j) <= com_rad:
                                    if t_d_j != 0:
                                        all_empty = False
                                elif distance(t_reg_i, t_reg_j) > com_rad:
                                    if t_d_j != f_d_j:
                                        all_empty = False
                            if ((all_empty) and (t_d_i == 0)):
                                e_weight = fts_i.edge[f_n_i][t_n_i]['weight']
                                if e_weight > a_weight:
                                    a_weight = e_weight
                            else:
                                allowed = False
                                break
                    else:
                        allowed = False
                        break
                if allowed:
                    comp_motion.add_edge(f_n, t_n, weight=a_weight)
    print 'Composed motion: nodes %d, edges %d' % (len(
        comp_motion.nodes()), len(comp_motion.edges()))
    # for e in comp_motion.edges():
    #if (e[1][0][0][0] == e[1][1][0][0]) and (e[1][1][0][1] == 'ul'):
    # if (e[0][1][0][1] == 'ul') and (e[0][0][1] == e[0][1][1] == 0):
    #     print e
    init_node = []
    for k in range(0, N):
        for nd in sys_models[k][0].graph['initial']:
            init_node.append((nd, 0))
    comp_motion.graph['initial'] = set([tuple(init_node)])
    #------------------------------
    comp_action_dict = dict()
    comp_action = ActionModel(comp_action_dict)
    comp_fts = MotActModel(comp_motion, comp_action)
    print '----------'
    print 'comp_fts generated in %.2f' % (time.time() - t0)
    print 'Composed fts: nodes %d, edges %d' % (len(
        comp_fts.nodes()), len(comp_fts.edges()))
    print '----------'
    return comp_fts
コード例 #11
0
def construct_sys_model_small(N):
    #----------------------------
    print 'Construct_agent_model_small starts'
    t0 = time.time()
    sys_models = []
    #---------- load simplified roadmap
    [c1_paths, c2_paths, c3_paths,
     l_paths] = pickle.load(open('simp_ts.p', 'rb'))
    #----------
    # N c1 agents, N c2 agents, N c3 agents, N leaders
    #----------
    A_start_pose = [(5.666666666666667, 5.0),
                    (4.666666666666667, 4.333333333333333),
                    (6.5, 6.666666666666667), (5.666666666666667, 5.0),
                    (4.666666666666667, 4.333333333333333),
                    (6.5, 6.666666666666667)]
    A_linear_speed = [0.45, 0.65, 1.35, 0.45, 0.65, 1.35]  #m/s
    A_angular_speed = [0.75, 0.9, 1.05, 0.75, 0.9, 1.05]  #rad/s
    COST = 1
    #----------
    add_data = {}
    act_time = {}
    symbols = []
    l_regions = set()
    #----------
    #----------
    #----------
    # group ---**c1**---
    for n in range(0, N):
        r_name = 'a1%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_symbols = ['r%d0' % n, 'r%d1' % n, 'r%d2' % n, 'r%d3' % n]
        symbols.append(r_symbols)
        r_regs = {
            r_init_pose: set([
                r_symbols[0],
            ]),
            (6.666666666666667, 9.5): set([
                r_symbols[1],
            ]),
            (1.0, 8.833333333333334): set([
                r_symbols[2],
            ]),
            (9.333333333333334, 9.0): set([
                r_symbols[3],
            ]),
        }
        r_motion = MotionFts(r_regs, r_symbols, '%s_FTS' % r_name)
        l_regions.update(set(r_regs.keys()))
        for pair, route in c1_paths.iteritems():
            if pair[0] in r_regs.keys():
                if pair[1] in r_regs.keys():
                    r_motion.add_edge(pair[0],
                                      pair[1],
                                      weight=route[1] / r_linear_speed)
        r_motion.set_initial(r_init_pose)

        # action fts
        add_data['g%d1' % n] = 2

        act_time['g%d1' % n] = 2 * COST
        symbols.append([
            'g%d1' % n,
        ])

        r_action_dict = {
            'g%d1' % n: (act_time['g%d1' % n], '1', set([
                'g%d1' % n,
            ])),
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = '([] <> (r%d1 && g%d1)) && ([] <> (r%d2 && g%d1)) && ([] <> (r%d3 && g%d1))' % tuple(
            (n, ) * 6)
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    #----------
    #----------
    #----------
    # group ---**c2**---
    for n in range(0, N):
        r_name = 'a2%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_symbols = ['r%d0' % n, 'r%d4' % n, 'r%d5' % n, 'r%d6' % n]
        symbols.append(r_symbols)
        r_regs = {
            r_init_pose: set([
                r_symbols[0],
            ]),
            (9.666666666666666, 6.0): set([
                r_symbols[1],
            ]),
            (8.666666666666666, 1.1666666666666667): set([
                r_symbols[2],
            ]),
            (4.666666666666667, 0.5): set([
                r_symbols[3],
            ]),
        }
        r_motion = MotionFts(r_regs, r_symbols, '%s_FTS' % r_name)
        l_regions.update(set(r_regs.keys()))
        for pair, route in c2_paths.iteritems():
            if pair[0] in r_regs.keys():
                if pair[1] in r_regs.keys():
                    r_motion.add_edge(pair[0],
                                      pair[1],
                                      weight=route[1] / r_linear_speed)
        r_motion.set_initial(r_init_pose)
        # action fts
        add_data['g%d4' % n] = 2
        act_time['g%d4' % n] = COST
        symbols.append([
            'g%d4' % n,
        ])

        r_action_dict = {
            'g%d4' % n: (act_time['g%d4' % n], '1', set([
                'g%d4' % n,
            ])),
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = '([] <> (r%d4 && g%d4)) && ([] <> (r%d6 && g%d4)) && ([] <> (r%d5 && g%d4))' % tuple(
            (n, ) * 6)
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    # #----------
    # #----------
    # #----------
    # # group ---**c3**---
    # for n in range(0, N):
    #     r_name  = 'a3%d' %n
    #     r_init_pose = A_start_pose[n]
    #     r_linear_speed = A_linear_speed[n]
    #     r_angular_speed = A_angular_speed[n]
    #     # motion fts
    #     r_symbols = ['r%d0' %n, 'r%d7' %n, 'r%d8' %n, 'r%d9' %n]
    #     symbols.append(r_symbols)
    #     r_regs = {r_init_pose: set([r_symbols[0],]),
    #               (0.3333333333333333, 5.666666666666667): set([r_symbols[1],]),
    #             (1.0, 3.3333333333333335): set([r_symbols[2],]),
    #             (4.333333333333333, 2.3333333333333335): set([r_symbols[3],]),
    #     }
    #     r_motion = MotionFts(r_regs, r_symbols, '%s_FTS' %r_name)
    #     l_regions.update(set(r_regs.keys()))
    #     for pair, route in c3_paths.iteritems():
    #         if pair[0] in r_regs.keys():
    #             if pair[1] in r_regs.keys():
    #                 r_motion.add_edge(pair[0], pair[1], weight = route[1]/r_linear_speed)
    #     r_motion.set_initial(r_init_pose)
    #     # action fts
    #     add_data['g%d6'%n] = 2

    #     act_time['g%d6'%n] = COST
    #     symbols.append(['g%d6'%n, ])

    #     r_action_dict = {
    #         'g%d6'%n: (act_time['g%d6'%n], '1', set(['g%d6'%n,])),
    #     }
    #     r_action = ActionModel(r_action_dict)
    #     r_model = MotActModel(r_motion, r_action)
    #     r_hard_task = '([] <> (r%d7 && g%d6)) && ([] <> (r%d8 && g%d6)) && ([] <> (r%d9 && g%d6))' %tuple((n,)*6)
    #     r_soft_task = None
    #     sys_models.append([r_model, r_hard_task, r_soft_task])
    #----------
    #----------
    #----------
    # group ---**leader**---
    for n in range(0, N):
        r_name = 'l%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_regs = dict()
        for nd in l_regions:
            r_regs[nd] = set()
        r_motion = MotionFts(r_regs, [], '%s_FTS' % r_name)
        for pair, route in l_paths.iteritems():
            if pair[0] in r_regs.keys():
                if pair[1] in r_regs.keys():
                    r_motion.add_edge(pair[0],
                                      pair[1],
                                      weight=route[1] / r_linear_speed)
        r_motion.set_initial(r_init_pose)
        # action fts
        add_data['ul'] = 0
        act_time['ul'] = COST
        symbols.append([
            'ul',
        ])
        r_action_dict = {
            'ul': (act_time['ul'], '1', set([
                'ul',
            ]))
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = 'true'
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    #--------------------
    print 'Agent model construction done in %.2f' % (time.time() - t0)
    print '%d source robots and %d relay robots' % (N, 3 * N)
    return sys_models, add_data, symbols
コード例 #12
0
def construct_sys_model(N):
    #----------------------------
    print 'Construct_agent_model starts'
    t0 = time.time()
    sys_models = []
    #----------
    [ws, tris, wps] = pickle.load(open('ws_model.p', 'rb'))
    # --- construct roadmap as fts ---
    roadmap = networkx.Graph()
    # --- use only center point, to reduce the number of nodes
    wpc = dict()
    print 'wps number', len(wps)
    for k, wp in enumerate(wps):
        roadmap.add_node(wp[0])
        wpc[wp[0]] = k
    for f_n in roadmap.nodes():
        for t_n in roadmap.nodes():
            if f_n != t_n:
                f_k = wpc[f_n]
                t_k = wpc[t_n]
                f_s = set(wps[f_k])
                t_s = set(wps[t_k])
                if f_s.intersection(t_s):
                    roadmap.add_edge(f_n, t_n, weight=distance(f_n, t_n))
    # if complete wps are used
    # for wp in wps:
    #     roadmap.add_edge(wp[0], wp[1], weight = distance(wp[0], wp[1]))
    #     roadmap.add_edge(wp[0], wp[2], weight = distance(wp[0], wp[2]))
    #     roadmap.add_edge(wp[0], wp[3], weight = distance(wp[0], wp[3]))
    # roadmap.add_edge(wp[1], wp[2], weight = distance(wp[1], wp[2]))
    # roadmap.add_edge(wp[1], wp[3], weight = distance(wp[1], wp[3]))
    # roadmap.add_edge(wp[2], wp[3], weight = distance(wp[2], wp[3]))
    #----------
    # N c1 agents, N c2 agents, N c3 agents, N leaders
    #----------
    A_start_pose = [(5.666666666666667, 5.0),
                    (4.666666666666667, 4.333333333333333),
                    (6.5, 6.666666666666667), (5.666666666666667, 5.0),
                    (4.666666666666667, 4.333333333333333),
                    (6.5, 6.666666666666667)]
    A_linear_speed = [0.45, 0.65, 1.35, 0.45, 0.65, 1.35]  #m/s
    A_angular_speed = [0.75, 0.9, 1.05, 0.75, 0.9, 1.05]  #rad/s
    COST = 1
    #----------
    add_data = {}
    act_time = {}
    symbols = []
    #----------
    #----------
    #----------
    # group ---**c1**---
    for n in range(0, N):
        r_name = 'a1%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_symbols = ['r%d0' % n, 'r%d1' % n, 'r%d2' % n, 'r%d3' % n]
        symbols.append(r_symbols)
        r_regs = {
            r_init_pose: set([
                r_symbols[0],
            ]),
            (6.666666666666667, 9.5): set([
                r_symbols[1],
            ]),
            (1.0, 8.833333333333334): set([
                r_symbols[2],
            ]),
            (9.333333333333334, 9.0): set([
                r_symbols[3],
            ]),
        }
        r_regions = {}
        for nd in roadmap.nodes():
            if nd in r_regs:
                r_regions[nd] = r_regs[nd]
            else:
                r_regions[nd] = set()
        r_motion = MotionFts(r_regions, r_symbols, '%s_FTS' % r_name)
        r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
        r_motion.set_initial(r_init_pose)

        # action fts
        add_data['g%d1' % n] = 1
        add_data['g%d2' % n] = 2
        add_data['g%d3' % n] = 2

        act_time['g%d1' % n] = COST
        act_time['g%d2' % n] = 2 * COST
        act_time['g%d3' % n] = COST
        symbols.append(['g%d1' % n, 'g%d2' % n, 'g%d3' % n])

        r_action_dict = {
            'g%d1' % n: (act_time['g%d1' % n], '1', set([
                'g%d1' % n,
            ])),
            'g%d2' % n: (act_time['g%d2' % n], '1', set([
                'g%d2' % n,
            ])),
            'g%d3' % n: (act_time['g%d3' % n], '1', set([
                'g%d3' % n,
            ])),
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = '([] <> (r%d1 && g%d1)) && ([] <> (r%d2 && g%d2)) && ([] <> (r%d3 && g%d3))' % tuple(
            (n, ) * 6)
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    #----------
    #----------
    #----------
    # group ---**c2**---
    for n in range(0, N):
        r_name = 'a2%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_symbols = ['r%d0' % n, 'r%d4' % n, 'r%d5' % n, 'r%d6' % n]
        symbols.append(r_symbols)
        r_regs = {
            r_init_pose: set([
                r_symbols[0],
            ]),
            (9.666666666666666, 6.0): set([
                r_symbols[1],
            ]),
            (8.666666666666666, 1.1666666666666667): set([
                r_symbols[2],
            ]),
            (4.666666666666667, 0.5): set([
                r_symbols[3],
            ]),
        }
        r_regions = {}
        for nd in roadmap.nodes():
            if nd in r_regs:
                r_regions[nd] = r_regs[nd]
            else:
                r_regions[nd] = set()
        r_motion = MotionFts(r_regions, r_symbols, '%s_FTS' % r_name)
        r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
        r_motion.set_initial(r_init_pose)
        # action fts
        add_data['g%d4' % n] = 2
        add_data['g%d5' % n] = 1

        act_time['g%d4' % n] = COST
        act_time['g%d5' % n] = COST
        symbols.append(['g%d4' % n, 'g%d5' % n])

        r_action_dict = {
            'g%d4' % n: (act_time['g%d4' % n], '1', set([
                'g%d4' % n,
            ])),
            'g%d5' % n: (act_time['g%d5' % n], '1', set([
                'g%d5' % n,
            ])),
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = '([] <> (r%d4 && g%d4)) && ([] <> (r%d6 && g%d4)) && ([] <> (r%d5 && g%d5))' % tuple(
            (n, ) * 6)
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    #----------
    #----------
    #----------
    # group ---**c3**---
    for n in range(0, N):
        r_name = 'a3%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_symbols = ['r%d0' % n, 'r%d7' % n, 'r%d8' % n, 'r%d9' % n]
        symbols.append(r_symbols)
        r_regs = {
            r_init_pose: set([
                r_symbols[0],
            ]),
            (0.3333333333333333, 5.666666666666667): set([
                r_symbols[1],
            ]),
            (1.0, 3.3333333333333335): set([
                r_symbols[2],
            ]),
            (4.333333333333333, 2.3333333333333335): set([
                r_symbols[3],
            ]),
        }
        r_regions = {}
        for nd in roadmap.nodes():
            if nd in r_regs:
                r_regions[nd] = r_regs[nd]
            else:
                r_regions[nd] = set()
        r_motion = MotionFts(r_regions, r_symbols, '%s_FTS' % r_name)
        r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
        r_motion.set_initial(r_init_pose)
        # action fts
        add_data['g%d6' % n] = 2
        add_data['g%d7' % n] = 1

        act_time['g%d6' % n] = COST
        act_time['g%d7' % n] = 2 * COST
        symbols.append(['g%d6' % n, 'g%d7' % n])

        r_action_dict = {
            'g%d6' % n: (act_time['g%d6' % n], '1', set([
                'g%d6' % n,
            ])),
            'g%d7' % n: (act_time['g%d7' % n], '1', set([
                'g%d7' % n,
            ])),
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = '([] <> (r%d7 && g%d6)) && ([] <> (r%d8 && g%d7)) && ([] <> (r%d9 && g%d6))' % tuple(
            (n, ) * 6)
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    #----------
    #----------
    #----------
    # group ---**leader**---
    for n in range(0, N):
        r_name = 'l%d' % n
        r_init_pose = A_start_pose[n]
        r_linear_speed = A_linear_speed[n]
        r_angular_speed = A_angular_speed[n]
        # motion fts
        r_regions = {}
        for nd in roadmap.nodes():
            r_regions[nd] = set()
        r_motion = MotionFts(r_regions, [], '%s_FTS' % r_name)
        r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
        r_motion.set_initial(r_init_pose)
        # action fts
        act_time['ul'] = COST
        symbols.append([
            'ul',
        ])
        r_action_dict = {
            'ul': (act_time['ul'], '1', set([
                'ul',
            ]))
        }
        r_action = ActionModel(r_action_dict)
        r_model = MotActModel(r_motion, r_action)
        r_hard_task = 'true'
        r_soft_task = None
        sys_models.append([r_model, r_hard_task, r_soft_task])
    #--------------------
    print 'Agent model construction done in %.2f' % (time.time() - t0)
    print '%d source robots and %d relay robots' % (N, 3 * N)
    return sys_models, add_data, symbols
コード例 #13
0
ファイル: agents_init.py プロジェクト: MengGuo/P_MAS_TG
#=================================
# A1 fts
A1_init_pose = [2.0,2.0,0.0]
A1_node_dict = {
 (2.0,2.0,0.1): set(['r0']),
 (0.5,0.5,0.2): set(['r1','a']),
 (3.5,0.5,0.2): set(['r2']),
 (0.5,2.0,0.2): set(['r3']),
 (3.5,2.0,0.2): set(['r4','b']),
 (0.5,3.3,0.2): set(['r5']),
 (3.5,3.3,0.2): set(['r6']),
 (2.0,1.0,0.3): set(['r7']),
 (2.0,3.0,0.3): set(['r8']),
}
A1_symbols = set(['r0','r1','r2','r3','r4','r5','r6','r7','r8','a','b'])
A1_motion = MotionFts(A1_node_dict, A1_symbols, 'A1_FTS')
A1_motion.set_initial(A1_init_pose)
A1_motion.add_full_edges(unit_cost = 1.0/SPEED[0])
# A1 action
A1_action_dict= {'la': (COST,'a',set([ 'la'])),
				'ua': (COST,'1',set([ 'ua'])),
				'lb': (COST,'b',set([ 'lb'])),
				'ub': (COST,'1',set([ 'ub'])),}
dep['lb']=set(['hb',])
dep['ub']=set(['hb',])
A1_action = ActionModel(A1_action_dict)
# A1 task
A1_hard_task = '<>(la && <>(ua && r2)) && <>(lb && <>(ub && r3)) && ([]<>r0)'
#A1_hard_task = '<> t1'
A1_soft_task = None
# A1 model, planner 
コード例 #14
0
ファイル: agent_init.py プロジェクト: ptajvar/P_MAS_TG
           (-0.00, 0.00, -2.6)]
OY_label = {
    OY_node[0]: set(['oyr0']),
    OY_node[1]: set([
        'oyr1',
    ]),
    OY_node[2]: set([
        'oyr2',
    ]),
    OY_node[3]: set([
        'oyr3',
    ]),
}
OY_init_pose = OY_node[0]
OY_symbols = set(['oyr0', 'oyr1', 'oyr2', 'oyr3'])
OY_motion = MotionFts(OY_label, OY_symbols, 'OY-workspace')
OY_motion.set_initial(OY_init_pose)
OY_edge = [
    (OY_node[0], OY_node[1]),
    (OY_node[1], OY_node[2]),
    (OY_node[2], OY_node[3]),
    (OY_node[1], OY_node[3]),
]
OY_motion.add_un_edges(OY_edge, unit_cost=10.0)
########### OY action ##########
OY_action_label = {
    'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])),
}
OY_action = ActionModel(OY_action_label)
########### OY task ############
#OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)'