-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathshortest_path_robot.py
85 lines (55 loc) · 3.07 KB
/
shortest_path_robot.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
import numpy as np
from point import *
import matplotlib.pyplot as plt
pw = PointWorldRobot(start_and_end = True, startp = Point(0, 5), endp = Point(100, 100))
# The first point has to (0,0) (relate to the start point), the reference point has to be the point with lowest y value, the order has to ccw
pw.define_robot_shape([Point(0, 0), Point(0, -5), Point(10, -5)])
pw.add_polygon(PointSet.from_list([Point(20, 20), Point(80, 20), Point(80, 80), Point(20, 80)]))
pw.show_polygon_world()
pw.update_new_polygon()
pw.show_shortest_path_only(show = False)
plt.savefig('figures/shortest_path_robot_1.png', bbox_inches = 'tight')
pw = PointWorldRobot(start_and_end = True, startp = Point(0, 5), endp = Point(100, 100))
# The first point has to (0,0) (relate to the start point), the reference point has to be the point with lowest y value, the order has to ccw
pw.define_robot_shape([Point(0, 0), Point(0, -5), Point(10, -5)])
pw.add_convex_polygon(10, 10, 20, 20, random_n = 5)
pw.add_convex_polygon(20, 20, 40, 40)
pw.add_convex_polygon(50, 30, 90, 70)
pw.show_polygon_world()
pw.update_new_polygon()
pw.show_shortest_path_only(show = False)
plt.savefig('figures/shortest_path_robot_2.png', bbox_inches = 'tight')
pw = PointWorldRobot(start_and_end = True, startp = Point(0, 5), endp = Point(100, 100))
# The first point has to (0,0) (relate to the start point), the reference point has to be the point with lowest y value, the order has to ccw
pw.define_robot_shape([Point(0, 0), Point(0, -5), Point(10, -5)])
pw.add_convex_polygon(10, 10, 20, 20, random_n = 5)
pw.add_convex_polygon(10, 10, 20, 20, random_n = 5)
pw.add_convex_polygon(10, 70, 30, 80, random_n = 5)
pw.add_convex_polygon(50, 30, 70, 40, random_n = 5)
pw.add_convex_polygon(90, 30, 100, 40, random_n = 5)
pw.add_convex_polygon(40, 60, 50, 70, random_n = 5)
pw.add_convex_polygon(10, 70, 20, 80, random_n = 5)
pw.add_convex_polygon(60, 30, 70, 40, random_n = 5)
pw.add_convex_polygon(20, 20, 40, 40)
pw.add_convex_polygon(50, 30, 90, 60)
pw.show_polygon_world()
pw.update_new_polygon()
pw.show_shortest_path_only(show = False)
plt.savefig('figures/shortest_path_robot_3.png', bbox_inches = 'tight')
pw = PointWorldRobot(start_and_end = True, startp = Point(0, 5), endp = Point(100, 100))
# The first point has to (0,0) (relate to the start point), the reference point has to be the point with lowest y value, the order has to ccw
pw.define_robot_shape([Point(0, 0), Point(0, -5), Point(10, -5)])
pw.add_convex_polygon(10, 10, 20, 50, random_n = 5)
pw.add_convex_polygon(10, 10, 40, 20, random_n = 5)
pw.add_convex_polygon(10, 70, 30, 80, random_n = 5)
pw.add_convex_polygon(50, 30, 90, 40, random_n = 5)
pw.add_convex_polygon(90, 30, 100, 40, random_n = 5)
pw.add_convex_polygon(40, 60, 80, 70, random_n = 5)
pw.add_convex_polygon(10, 70, 50, 80, random_n = 5)
pw.add_convex_polygon(60, 30, 70, 40, random_n = 5)
pw.add_convex_polygon(20, 20, 40, 40)
pw.add_convex_polygon(50, 30, 90, 60)
pw.show_polygon_world()
pw.update_new_polygon()
pw.show_shortest_path_only(show = False)
plt.savefig('figures/shortest_path_robot_4.png', bbox_inches = 'tight')