Newer
Older
#! /usr/bin/env python
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
from guided_mrmp.controllers.utils import compute_path_from_wp, get_ref_trajectory
from guided_mrmp.controllers.mpc import MPC
from guided_mrmp.utils import Roomba
T = 1 # Prediction Horizon [s]
DT = 0.2 # discretization step [s]
# Classes
def __init__(self, initial_position, target_v, T, DT, waypoints):
"""
waypoints (list [[xpoints],[ypoints]]):
"""
self.state = initial_position
self.T = T
self.DT = DT
self.target_v = target_v
# helper variable to keep track of mpc output
# starting condition is 0,0
self.control = np.zeros(2)
self.K = int(T / DT)
# For a car model
# Q = [20, 20, 10, 20] # state error cost
# Qf = [30, 30, 30, 30] # state final error cost
# R = [10, 10] # input cost
# P = [10, 10] # input rate of change cost
# self.mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P)
# For a circular robot (easy dynamics)
Q = [20, 20, 20] # state error cost
Qf = [30, 30, 30] # state final error cost
R = [10, 10] # input cost
P = [10, 10] # input rate of change cost
self.mpc = MPC(Roomba(), T, DT, Q, Qf, R, P)
# Path from waypoint interpolation
self.path = compute_path_from_wp(waypoints[0], waypoints[1], 0.05)
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
# Helper variables to keep track of the sim
self.sim_time = 0
self.x_history = []
self.y_history = []
self.v_history = []
self.h_history = []
self.a_history = []
self.d_history = []
self.optimized_trajectory = None
# Initialise plot
# plt.style.use("ggplot")
# self.fig = plt.figure()
# plt.ion()
# plt.show()
def ego_to_global(self, mpc_out):
"""
transforms optimized trajectory XY points from ego(car) reference
into global(map) frame
Args:
mpc_out ():
"""
trajectory = np.zeros((2, self.K))
trajectory[:, :] = mpc_out[0:2, 1:]
Rotm = np.array(
[
[np.cos(self.state[3]), np.sin(self.state[3])],
[-np.sin(self.state[3]), np.cos(self.state[3])],
]
)
trajectory = (trajectory.T.dot(Rotm)).T
trajectory[0, :] += self.state[0]
trajectory[1, :] += self.state[1]
return trajectory
def ego_to_global_roomba(self, mpc_out):
"""
Transforms optimized trajectory XY points from ego (robot) reference
into global (map) frame.
Args:
mpc_out (numpy array): Optimized trajectory points in ego reference frame.
Returns:
numpy array: Transformed trajectory points in global frame.
"""
# Extract x, y, and theta from the state
x = self.state[0]
y = self.state[1]
theta = self.state[2]
# Rotation matrix to transform points from ego frame to global frame
Rotm = np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]
])
# Initialize the trajectory array (only considering XY points)
trajectory = mpc_out[0:2, :]
# Apply rotation to the trajectory points
trajectory = Rotm.dot(trajectory)
# Translate the points to the robot's position in the global frame
trajectory[0, :] += x
trajectory[1, :] += y
return trajectory
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
def get_next_control(self, state, show_plots=False):
# optimization loop
# start=time.time()
# Get Reference_traj -> inputs are in worldframe
target = get_ref_trajectory(np.array(state), np.array(self.path), self.target_v, self.T, self.DT)
# dynamycs w.r.t robot frame
# curr_state = np.array([0, 0, self.state[2], 0])
curr_state = np.array([0, 0, 0])
x_mpc, u_mpc = self.mpc.step(
curr_state,
target,
self.control
)
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
# only the first one is used to advance the simulation
self.control[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
# self.state = self.predict_next_state(
# self.state, [self.control[0], self.control[1]], DT
# )
return self.control
def run(self, show_plots=False):
"""
[TODO:summary]
[TODO:description]
"""
if show_plots: self.plot_sim()
self.x_history.append(self.state[0])
self.y_history.append(self.state[1])
self.h_history.append(self.state[2])
while 1:
if (np.sqrt((self.state[0] - self.path[0, -1]) ** 2 + (self.state[1] - self.path[1, -1]) ** 2) < 0.1):
print("Success! Goal Reached")
return np.asarray([self.x_history, self.y_history, self.h_history])
controls = self.get_next_control()
next_state = self.predict_next_state_roomba(self.state, [self.control[0], self.control[1]], self.DT)
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
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
# use the optimizer output to preview the predicted state trajectory
# self.optimized_trajectory = self.ego_to_global(x_mpc.value)
if show_plots: self.optimized_trajectory = self.ego_to_global_roomba(x_mpc.value)
if show_plots: self.plot_sim()
self.x_history.append(self.state[0])
self.y_history.append(self.state[1])
self.h_history.append(self.state[2])
def predict_next_state_roomba(self, state, u, dt):
dxdt = u[0] * np.cos(state[2])
dydt = u[0] * np.sin(state[2])
dthetadt = u[1]
# Update state using Euler integration
new_x = state[0] + dxdt * dt
new_y = state[1] + dydt * dt
new_theta = state[2] + dthetadt * dt
# Return the predicted next state
return np.array([new_x, new_y, new_theta])
def plot_sim(self):
self.sim_time = self.sim_time + self.DT
# self.x_history.append(self.state[0])
# self.y_history.append(self.state[1])
# self.v_history.append(self.control[0])
self.h_history.append(self.state[2])
self.d_history.append(self.control[1])
plt.clf()
grid = plt.GridSpec(2, 3)
plt.subplot(grid[0:2, 0:2])
plt.title(
"MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time)
)
plt.plot(
self.path[0, :],
self.path[1, :],
c="tab:orange",
marker=".",
label="reference track",
)
plt.plot(
self.x_history,
self.y_history,
c="tab:blue",
marker=".",
alpha=0.5,
label="vehicle trajectory",
)
if self.optimized_trajectory is not None:
plt.plot(
self.optimized_trajectory[0, :],
self.optimized_trajectory[1, :],
c="tab:green",
marker="+",
alpha=0.5,
label="mpc opt trajectory",
)
# plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
# marker=".",
# markersize=12,
# label="vehicle position")
# plt.arrow(self.x_history[-1],
# self.y_history[-1],
# np.cos(self.h_history[-1]),
# np.sin(self.h_history[-1]),
# color='tab:blue',
# width=0.2,
# head_length=0.5,
# label="heading")
# plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
plot_roomba(self.x_history[-1], self.y_history[-1], self.h_history[-1])
plt.ylabel("map y")
plt.yticks(
np.arange(min(self.path[1, :]) - 1.0, max(self.path[1, :] + 1.0) + 1, 1.0)
)
plt.xlabel("map x")
plt.xticks(
np.arange(min(self.path[0, :]) - 1.0, max(self.path[0, :] + 1.0) + 1, 1.0)
)
plt.axis("equal")
# plt.legend()
plt.subplot(grid[0, 2])
# plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
# plt.plot(self.a_history, c="tab:orange")
# locs, _ = plt.xticks()
# plt.xticks(locs[1:], locs[1:] * DT)
# plt.ylabel("a(t) [m/ss]")
# plt.xlabel("t [s]")
plt.subplot(grid[1, 2])
# plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
plt.plot(np.degrees(self.d_history), c="tab:orange")
plt.ylabel("gamma(t) [deg]")
locs, _ = plt.xticks()
plt.xticks(locs[1:], locs[1:] * DT)
plt.xlabel("t [s]")
plt.tight_layout()
plt.draw()
plt.pause(0.1)
def plot_roomba(x, y, yaw):
"""
Args:
x ():
y ():
yaw ():
"""
LENGTH = 0.5 # [m]
WIDTH = 0.25 # [m]
OFFSET = LENGTH # [m]
fig = plt.gcf()
ax = fig.gca()
circle = plt.Circle((x, y), .5, color='b', fill=False)
ax.add_patch(circle)
# Plot direction marker
dx = 1 * np.cos(yaw)
dy = 1 * np.sin(yaw)
ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')
if __name__ == "__main__":
# Example usage
initial_pos = np.array([0.0, 0.5, 0.0, 0.0])
target_vocity = 3.0 # m/s
T = 1 # Prediction Horizon [s]
DT = 0.2 # discretization step [s]
wp = [[0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0],
[0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2]]
sim = PathTracker(initial_position=initial_pos, target_v=target_vocity, T=T, DT=DT, waypoints=wp)