12345qiupeng
/
Multi-Purpose-MPC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

MPC updates current waypoint and transforms temporal state into spatial 

state before computing current control signal
master
matssteinweg 2019-12-02 00:12:22 +01:00
parent e6f006e515
commit 447bdf9f41
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
MPC.py
View File

@ -2,6 +2,7 @@ import numpy as np
import osqp import osqp
from scipy import sparse from scipy import sparse
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from time import time
# Colors # Colors
PREDICTION = '#BA4A00' PREDICTION = '#BA4A00'
@ -145,6 +146,12 @@ class MPC:
# Number of state variables # Number of state variables
nx = self.model.n_states nx = self.model.n_states
# Update current waypoint
self.model.get_current_waypoint()
# Update spatial state
self.model.spatial_state = self.model.t2s()
# Initialize optimization problem # Initialize optimization problem
self._init_problem(v) self._init_problem(v)
@ -198,17 +205,18 @@ class MPC:
x_pred, y_pred = [], [] x_pred, y_pred = [], []
# get current waypoint ID # get current waypoint ID
print('#########################') #print('#########################')
for n in range(2, self.N): for n in range(2, self.N):
associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
predicted_temporal_state = self.model.s2t(associated_waypoint, predicted_temporal_state = self.model.s2t(associated_waypoint,
spatial_state_prediction[n, :]) spatial_state_prediction[n, :])
print('delta: ', u)
print('e_y: ', spatial_state_prediction[n, 0]) #print('delta: ', u)
print('e_psi: ', spatial_state_prediction[n, 1]) #print('e_y: ', spatial_state_prediction[n, 0])
print('t: ', spatial_state_prediction[n, 2]) #print('e_psi: ', spatial_state_prediction[n, 1])
print('+++++++++++++++++++++++') #print('t: ', spatial_state_prediction[n, 2])
#print('+++++++++++++++++++++++')
x_pred.append(predicted_temporal_state.x) x_pred.append(predicted_temporal_state.x)
y_pred.append(predicted_temporal_state.y) y_pred.append(predicted_temporal_state.y)