-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathstl.py
95 lines (79 loc) · 3.01 KB
/
stl.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
from matplotlib import pyplot as plt
from stl_mob.stl.solver import StlpySolver
from stl_mob.stl.tasks import BranchTask, CoverTask, LoopTask, Map, SequenceTask, SignalTask, AltCoverTask
def test_seq_task_sol():
task_map = Map.generate_map(4, (-5, 5), ((1, 1), (2, 2)))
task = SequenceTask.generate_task(task_map, 4, total_time_steps=10)
spec = task.get_spec().get_stlpy_form()
solver = StlpySolver(space_dim=2)
path, info = solver.solve_stlpy_formula(
spec, task.sample_init_pos(), task.total_time_steps)
print(path)
print(task)
task.plot_solution(path)
plt.show()
print(task.get_quantitative_info())
def test_cover_task_sol():
task_map = Map.generate_map(4, (-5, 5), ((1, 1), (2, 2)))
task = CoverTask.generate_task(task_map, 4, total_time_steps=10)
spec = task.get_spec().get_stlpy_form()
solver = StlpySolver(space_dim=2)
path, info = solver.solve_stlpy_formula(
spec, task.sample_init_pos(), task.total_time_steps)
print(path)
print(task)
task.plot_solution(path)
plt.show()
def test_branch_task_sol():
task_map = Map.generate_map(4, (-5, 5), ((1, 1), (2, 2)))
task = BranchTask.generate_task(task_map, 4, total_time_steps=16)
spec = task.get_spec().get_stlpy_form()
solver = StlpySolver(space_dim=2)
path, info = solver.solve_stlpy_formula(
spec, task.sample_init_pos(), task.total_time_steps)
print(path)
print(task)
task.plot_solution(path)
plt.show()
def test_loop_task_sol():
task_map = Map.generate_map(4, (-5, 5), ((1, 1), (2, 2)))
task = LoopTask.generate_task(task_map, 4, total_time_steps=16)
spec = task.get_spec().get_stlpy_form()
solver = StlpySolver(space_dim=2)
path, info = solver.solve_stlpy_formula(
spec, task.sample_init_pos(), task.total_time_steps)
print(path)
print(task)
task.plot_solution(path)
plt.show()
def test_signal_task_sol():
task_map = Map.generate_map(4, (-5, 5), ((1, 1), (2, 2)))
task = SignalTask.generate_task(task_map, 6, total_time_steps=8)
spec = task.get_spec().get_stlpy_form()
solver = StlpySolver(space_dim=2)
path, info = solver.solve_stlpy_formula(
spec, task.sample_init_pos(), task.total_time_steps)
print(f"the last second waypoint is: {task.wp_list[-2]}")
print(f"the last waypoint is: {task.wp_list[-1]}")
print(path)
print(task)
task.plot_solution(path)
plt.show()
def test_alt_cover_task_sol():
task_map = Map.generate_map(1, (-5, 5), ((1, 1), (2, 2)))
task = AltCoverTask.generate_task(task_map, 3, total_time_steps=15)
spec = task.get_spec().get_stlpy_form()
solver = StlpySolver(space_dim=2)
path, info = solver.solve_stlpy_formula(
spec, task.sample_init_pos(), task.total_time_steps)
print(path)
print(task)
task.plot_solution(path)
plt.show()
if __name__ == "__main__":
test_seq_task_sol()
test_cover_task_sol()
test_branch_task_sol()
test_loop_task_sol()
test_signal_task_sol()
test_alt_cover_task_sol()