From 3c9152ffab9969a4ff0c17a141de12d6cde8b461 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Tue, 14 Jan 2020 13:37:28 +0000 Subject: [PATCH] WIP: added heading arrow to simulation --- mpc_demo/main.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/mpc_demo/main.py b/mpc_demo/main.py index 43e071a..2f642ae 100755 --- a/mpc_demo/main.py +++ b/mpc_demo/main.py @@ -10,6 +10,7 @@ from cvxpy_mpc import optimize import sys import time +# Robot Starting position SIM_START_X=0 SIM_START_Y=2 SIM_START_H=0 @@ -39,6 +40,7 @@ class MPC(): self.sim_time=0 self.x_history=[] self.y_history=[] + self.h_history=[] self.v_history=[] self.w_history=[] @@ -84,32 +86,36 @@ class MPC(): self.sim_time = self.sim_time+self.dt self.x_history.append(self.state[0]) self.y_history.append(self.state[1]) + self.h_history.append(self.state[2]) self.v_history.append(self.opt_u[0,1]) self.w_history.append(self.opt_u[1,1]) plt.clf() - plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time)) 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), - # ylim=(np.min(self.path[1,:])-1, np.max(self.path[1,:])+1)) + 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") 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.xlabel('map x') plt.legend() 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.ylabel('v(t) [m/s]') + plt.xlabel('sample') 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.ylabel('w(t) [deg/s]') + plt.xlabel('sample') plt.tight_layout()