-
Notifications
You must be signed in to change notification settings - Fork 20
/
assignment3.py
35 lines (27 loc) · 801 Bytes
/
assignment3.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
import numpy as np
from sim.sim2d import sim_run
# Simulator options.
options = {}
options['FIG_SIZE'] = [8,8]
options['OBSTACLES'] = True
class ModelPredictiveControl:
def __init__(self):
self.horizon = 2
self.dt = 0.2
# Reference or set point the controller will achieve.
self.reference1 = [10, 0, 0]
self.reference2 = None
self.x_obs = 5
self.y_obs = 0.1
def plant_model(self,prev_state, dt, pedal, steering):
x_t = prev_state[0]
y_t = prev_state[1]
psi_t = prev_state[2]
v_t = prev_state[3]
return [x_t, y_t, psi_t, v_t]
def cost_function(self,u, *args):
state = args[0]
ref = args[1]
cost = 0.0
return cost
sim_run(options, ModelPredictiveControl)