12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

WIP: added heading arrow to simulation

master
mcarfagno 2020-01-14 13:37:28 +00:00
parent f30abf18bb
commit 3c9152ffab
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
mpc_demo/main.py
View File

@ -10,6 +10,7 @@ from cvxpy_mpc import optimize
import sys import sys
import time import time
# Robot Starting position
SIM_START_X=0 SIM_START_X=0
SIM_START_Y=2 SIM_START_Y=2
SIM_START_H=0 SIM_START_H=0
@ -39,6 +40,7 @@ class MPC():
self.sim_time=0 self.sim_time=0
self.x_history=[] self.x_history=[]
self.y_history=[] self.y_history=[]
self.h_history=[]
self.v_history=[] self.v_history=[]
self.w_history=[] self.w_history=[]
@ -84,32 +86,36 @@ class MPC():
self.sim_time = self.sim_time+self.dt self.sim_time = self.sim_time+self.dt
self.x_history.append(self.state[0]) self.x_history.append(self.state[0])
self.y_history.append(self.state[1]) self.y_history.append(self.state[1])
self.h_history.append(self.state[2])
self.v_history.append(self.opt_u[0,1]) self.v_history.append(self.opt_u[0,1])
self.w_history.append(self.opt_u[1,1]) self.w_history.append(self.opt_u[1,1])
plt.clf() plt.clf()
plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time))
grid = plt.GridSpec(2, 3) grid = plt.GridSpec(2, 3)
plt.subplot(grid[0:2, 0:2])
# self.ax = plt.axes(xlim=(np.min(self.path[0,:])-1, np.max(self.path[0,:])+1), plt.subplot(grid[0:2, 0:2])
# ylim=(np.min(self.path[1,:])-1, np.max(self.path[1,:])+1)) 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.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") plt.plot(self.x_history, self.y_history, c='tab:blue', marker=".", alpha=0.5, label="vehicle trajectory")
plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue', marker=".", markersize=12, label="vehicle position") 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)
plt.ylabel('map y') plt.ylabel('map y')
plt.xlabel('map x') plt.xlabel('map x')
plt.legend() plt.legend()
plt.subplot(grid[0, 2]) plt.subplot(grid[0, 2])
#plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
plt.plot(self.v_history,c='tab:orange') plt.plot(self.v_history,c='tab:orange')
plt.ylabel('v(t) [m/s]') plt.ylabel('v(t) [m/s]')
plt.xlabel('sample')
plt.subplot(grid[1, 2]) plt.subplot(grid[1, 2])
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
plt.plot(np.degrees(self.w_history),c='tab:orange') plt.plot(np.degrees(self.w_history),c='tab:orange')
plt.ylabel('w(t) [deg/s]') plt.ylabel('w(t) [deg/s]')
plt.xlabel('sample')
plt.tight_layout() plt.tight_layout()