-
-
Notifications
You must be signed in to change notification settings - Fork 6.7k
/
Copy pathmove_to_pose_robot.py
240 lines (188 loc) · 7.21 KB
/
move_to_pose_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
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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
"""
Move to specified pose (with Robot class)
Author: Daniel Ingram (daniel-s-ingram)
Atsushi Sakai (@Atsushi_twi)
Seied Muhammad Yazdian (@Muhammad-Yazdian)
P.I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
"""
import matplotlib.pyplot as plt
import numpy as np
import copy
from move_to_pose import PathFinderController
# Simulation parameters
TIME_DURATION = 1000
TIME_STEP = 0.01
AT_TARGET_ACCEPTANCE_THRESHOLD = 0.01
SHOW_ANIMATION = True
PLOT_WINDOW_SIZE_X = 20
PLOT_WINDOW_SIZE_Y = 20
PLOT_FONT_SIZE = 8
simulation_running = True
all_robots_are_at_target = False
class Pose:
"""2D pose"""
def __init__(self, x, y, theta):
self.x = x
self.y = y
self.theta = theta
class Robot:
"""
Constructs an instantiate of the 3-DOF wheeled Robot navigating on a
2D plane
Parameters
----------
name : (string)
The name of the robot
color : (string)
The color of the robot
max_linear_speed : (float)
The maximum linear speed that the robot can go
max_angular_speed : (float)
The maximum angular speed that the robot can rotate about its vertical
axis
path_finder_controller : (PathFinderController)
A configurable controller to finds the path and calculates command
linear and angular velocities.
"""
def __init__(self, name, color, max_linear_speed, max_angular_speed,
path_finder_controller):
self.name = name
self.color = color
self.MAX_LINEAR_SPEED = max_linear_speed
self.MAX_ANGULAR_SPEED = max_angular_speed
self.path_finder_controller = path_finder_controller
self.x_traj = []
self.y_traj = []
self.pose = Pose(0, 0, 0)
self.pose_start = Pose(0, 0, 0)
self.pose_target = Pose(0, 0, 0)
self.is_at_target = False
def set_start_target_poses(self, pose_start, pose_target):
"""
Sets the start and target positions of the robot
Parameters
----------
pose_start : (Pose)
Start position of the robot (see the Pose class)
pose_target : (Pose)
Target position of the robot (see the Pose class)
"""
self.pose_start = copy.copy(pose_start)
self.pose_target = pose_target
self.pose = pose_start
def move(self, dt):
"""
Moves the robot for one time step increment
Parameters
----------
dt : (float)
time step
"""
self.x_traj.append(self.pose.x)
self.y_traj.append(self.pose.y)
rho, linear_velocity, angular_velocity = \
self.path_finder_controller.calc_control_command(
self.pose_target.x - self.pose.x,
self.pose_target.y - self.pose.y,
self.pose.theta, self.pose_target.theta)
if rho < AT_TARGET_ACCEPTANCE_THRESHOLD:
self.is_at_target = True
if abs(linear_velocity) > self.MAX_LINEAR_SPEED:
linear_velocity = (np.sign(linear_velocity)
* self.MAX_LINEAR_SPEED)
if abs(angular_velocity) > self.MAX_ANGULAR_SPEED:
angular_velocity = (np.sign(angular_velocity)
* self.MAX_ANGULAR_SPEED)
self.pose.theta = self.pose.theta + angular_velocity * dt
self.pose.x = self.pose.x + linear_velocity * \
np.cos(self.pose.theta) * dt
self.pose.y = self.pose.y + linear_velocity * \
np.sin(self.pose.theta) * dt
def run_simulation(robots):
"""Simulates all robots simultaneously"""
global all_robots_are_at_target
global simulation_running
robot_names = []
for instance in robots:
robot_names.append(instance.name)
time = 0
while simulation_running and time < TIME_DURATION:
time += TIME_STEP
robots_are_at_target = []
for instance in robots:
if not instance.is_at_target:
instance.move(TIME_STEP)
robots_are_at_target.append(instance.is_at_target)
if all(robots_are_at_target):
simulation_running = False
if SHOW_ANIMATION:
plt.cla()
plt.xlim(0, PLOT_WINDOW_SIZE_X)
plt.ylim(0, PLOT_WINDOW_SIZE_Y)
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.text(0.3, PLOT_WINDOW_SIZE_Y - 1,
'Time: {:.2f}'.format(time),
fontsize=PLOT_FONT_SIZE)
plt.text(0.3, PLOT_WINDOW_SIZE_Y - 2,
'Reached target: {} = '.format(robot_names)
+ str(robots_are_at_target),
fontsize=PLOT_FONT_SIZE)
for instance in robots:
plt.arrow(instance.pose_start.x,
instance.pose_start.y,
np.cos(instance.pose_start.theta),
np.sin(instance.pose_start.theta),
color='r',
width=0.1)
plt.arrow(instance.pose_target.x,
instance.pose_target.y,
np.cos(instance.pose_target.theta),
np.sin(instance.pose_target.theta),
color='g',
width=0.1)
plot_vehicle(instance.pose.x,
instance.pose.y,
instance.pose.theta,
instance.x_traj,
instance.y_traj, instance.color)
plt.pause(TIME_STEP)
def plot_vehicle(x, y, theta, x_traj, y_traj, color):
# Corners of triangular vehicle when pointing to the right (0 radians)
p1_i = np.array([0.5, 0, 1]).T
p2_i = np.array([-0.5, 0.25, 1]).T
p3_i = np.array([-0.5, -0.25, 1]).T
T = transformation_matrix(x, y, theta)
p1 = T @ p1_i
p2 = T @ p2_i
p3 = T @ p3_i
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], color+'-')
plt.plot([p2[0], p3[0]], [p2[1], p3[1]], color+'-')
plt.plot([p3[0], p1[0]], [p3[1], p1[1]], color+'-')
plt.plot(x_traj, y_traj, color+'--')
def transformation_matrix(x, y, theta):
return np.array([
[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y],
[0, 0, 1]
])
def main():
pose_target = Pose(15, 15, -1)
pose_start_1 = Pose(5, 2, 0)
pose_start_2 = Pose(5, 2, 0)
pose_start_3 = Pose(5, 2, 0)
controller_1 = PathFinderController(5, 8, 2)
controller_2 = PathFinderController(5, 16, 4)
controller_3 = PathFinderController(10, 25, 6)
robot_1 = Robot("Yellow Robot", "y", 12, 5, controller_1)
robot_2 = Robot("Black Robot", "k", 16, 5, controller_2)
robot_3 = Robot("Blue Robot", "b", 20, 5, controller_3)
robot_1.set_start_target_poses(pose_start_1, pose_target)
robot_2.set_start_target_poses(pose_start_2, pose_target)
robot_3.set_start_target_poses(pose_start_3, pose_target)
robots: list[Robot] = [robot_1, robot_2, robot_3]
run_simulation(robots)
if __name__ == '__main__':
main()