-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathpath_experiment.py
125 lines (99 loc) · 3.48 KB
/
path_experiment.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
from PATH_PLAN.P_MAS_TG.ts import MotionFts, ActionModel, MotActModel
from PATH_PLAN.P_MAS_TG.planner import ltl_planner
from point_location import pl
from Controller_main import PID
import paho.mqtt.client as mqtt
import logging
import time
import sys
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(sys.argv[0])
##############################
# motion FTS
ap = {'r1', 'r2', 'r3', 'r4', 'r5', 'r6', 'r', 'b'}
# +-----+-----+-----+
# | r4,r| r5,b| r6,b|
# +-----+-----+-----+
# | r1,r| r2,b| r3,r|
# +-----+-----+-----+
regions = { (0, 0, 1): set(['r1', 'r']),
(1, 0, 1): set(['r2', 'b']),
(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)
##############################
# complete robot model
robot_model = MotActModel(robot_motion, robot_action)
##############################
# specify tasks
########## only hard
#hard_task = '<>(r1 && <> (r2 && <> r6)) && (<>[] r6)'
#hard_task = '<>(r1 && <> (r2 && <> r6)) && ([]<> r6) && ([]<> r1) && [] <>(r1 -> X r5)'
soft_task = None
########## soft and hard
hard_task = '(([]<> r3) && ([]<> r4)) '
# soft_task = '([]! b)'
##############################
# set planner
robot_planner = ltl_planner(robot_model, hard_task, soft_task)
# synthesis
start = time.time()
robot_planner.optimal(10,'static')
# the prefix of plan **aps**:
# [{'r', 'r1'}, {'r2', 'b'}, {'r', 'r3'}, {'r2', 'b'}, {'r', 'r1'}, {'r', 'r4'}, {'r', 'r4'}]
# the suffix of plan **aps**:
# [{'r', 'r4'}, {'r5', 'b'}, {'r6', 'b'}, {'r', 'r3'}, {'r2', 'b'}, {'r', 'r1'}, {'r', 'r4'}]
prefix = [robot_planner.product.graph['ts'].node[n]['label'] for n in robot_planner.run.line]
suffix = [robot_planner.product.graph['ts'].node[n]['label'] for n in robot_planner.run.loop]
def parse_nodes(path):
nodes = []
for point in path:
point.discard("r")
point.discard("b")
for i in point:
nodes.append(i)
return nodes
prefix_path = parse_nodes(prefix)
# mqtt_client
client = mqtt.Client()
for node in prefix_path:
logger.info("Now moving to {}, {}".format(pl[node][0], pl[node][1]))
controller = PID(v_const=0.003, d_stop=0.1, logger=logger)
controller.update_goal(x_goal=pl[node][0], y_goal=pl[node][1])
controller.start()
if node == "r3" or node == "r4":
controller.rotate(3)
suffix_path = parse_nodes(suffix)
# mqtt_client
client = mqtt.Client()
while True:
for node in suffix_path:
logger.info("Now moving to {}, {}".format(pl[node][0], pl[node][1]))
controller = PID(v_const=0.004, d_stop=0.1, logger=logger)
controller.update_goal(x_goal=pl[node][0], y_goal=pl[node][1])
controller.start()
if node == "r3" or node == "r4":
controller.rotate(3)
logger.info('full construction and synthesis done within %.2fs \n' %(time.time()-start))