-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathtry2.py
95 lines (90 loc) · 3.92 KB
/
try2.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
import LMA_VNS_APF
from matplotlib import pyplot as plt
a = [(0.1414213562373095, 0.1414213562373095),
(0.282842712474619, 0.282842712474619),
(0.42426406871192857, 0.42426406871192857),
(0.5656854249492381, 0.5656854249492381),
(0.7071067811865477, 0.7071067811865477),
(0.8485281374238571, 0.8485281374238571),
(0.9899494936611667, 0.9899494936611667),
(1.1313708498984762, 1.1313708498984762),
(1.1467142671161288, 1.3307814302298244),
(0.9539716970112928, 1.2773932136517967),
(1.0979433320918566, 1.4162174488999297),
(0.898089321817483, 1.42385777462928),
(1.0436523701196438, 1.5610124281011284),
(0.8994080398186908, 1.6995532999622132),
(1.047176181769764, 1.8343293224006552),
(1.1949443237208373, 1.9691053448390972),
(1.3427124656719105, 2.1038813672775394),
(1.4904806076229837, 2.2386573897159816),
(1.638248749574057, 2.373433412154424),
(1.7860168915251302, 2.508209434592866),
(1.9337850334762035, 2.6429854570313083),
(2.0815531754272767, 2.7777614794697505),
(2.22932131737835, 2.9125375019081927),
(2.377089459329423, 3.047313524346635),
(2.5248576012804964, 3.182089546785077),
(2.6726257432315697, 3.3168655692235194),
(2.820393885182643, 3.4516415916619616),
(2.968162027133716, 3.586417614100404),
(3.1159301690847894, 3.721193636538846),
(3.2636983110358626, 3.8559696589772883),
(3.411466452986936, 3.9907456814157305),
(3.559234594938009, 4.125521703854172),
(3.6263705729368487, 3.9371264797689016),
(3.771280449895397, 4.074971057318613),
(3.9373441333231876, 4.1864289105475105),
(4.081699828932904, 4.324853739046718),
(4.226055524542621, 4.463278567545926),
(4.370411220152337, 4.601703396045133),
(4.514766915762054, 4.740128224544341),
(4.65912261137177, 4.878553053043548),
(4.803478306981487, 5.016977881542756),
(4.947834002591203, 5.1554027100419635),
(5.09218969820092, 5.293827538541171), #!--------
(5.236545393810636, 5.432252367040379),
(5.380901089420353, 5.570677195539586),
(5.329500480553629, 5.3773950768422525),
(5.471648803232563, 5.518085710359788),
(5.3743532079583165, 5.343347142378274),
(5.5153013820041705, 5.485240102856022),
(5.351289925421049, 5.370783813778126),
(5.4930081098244985, 5.511907717527242),
(5.3680078925502865, 5.355782941526122),
(5.509242749449027, 5.497390551479798),
(5.358398924912184, 5.366065989424186),
(5.499937130274767, 5.507370399909986),
(5.36476963179401, 5.359960266762855),
(5.506117639960636, 5.501454933048647),
(5.3609085338874705, 5.363925607036221),
(5.502375884898823, 5.50530095353564), #!-----------
(5.363383972533436, 5.3614911892739165),
(5.504776465840319, 5.502941402552168),
(5.361851917855554, 5.363039388330248),
(5.503291378779192, 5.504442637563179)]
start = (0, 0)
goal = (10, 10)
obstacle_List1 = [[6, 6]]
obstacle_List2 = [[6, 6], [5.75, 6], [5.5, 6], [6, 5.75], [6, 5.5]]
obstacle_List3 = [[6, 6], [5.75, 6], [6, 5.75]]
obstacle_List4 = [[0.5, 1], [2.36, 1.7], [2.6, 3.2], [4, 3.5]]
k_att = 1
k_rep = 20
rep_range = 0.6
step_size = 0.2
max_iters = 1000
goal_threshold = 1.0
length = 18
num_sub = 4
neighbour_name = ["neighbourhood_random_eight", "neighbourhood_random"]
APF_try = LMA_VNS_APF.APF_VNS(start, goal, obstacle_List4, k_att, k_rep, rep_range, step_size, max_iters, goal_threshold, length, num_sub, neighbour_name)
APF_try.path = a
APF_try.dividePath2()
print(APF_try.subgoals)
fig = plt.figure(figsize=(10, 10))
subplot = fig.add_subplot(111) # 子图1：显示path和最后一个subgoals
subplot.set_xlabel('X')
subplot.set_ylabel('Y')
subplot.plot([p[0] for p in APF_try.subgoals], [p[1] for p in APF_try.subgoals], linestyle=':',marker='o', color = 'g',label = "first stuck subgoals", alpha = 1)
plt.show()