Skip to content

Commit 2bd2598

Browse files
author
Shunichi09
authored
Add unconstrained NMPC (#12)
* Add NMPC * Update README
1 parent 0d443f7 commit 2bd2598

14 files changed

Lines changed: 361 additions & 35 deletions

File tree

‎PythonLinearNonlinearControl/configs/first_order_lag.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,15 +52,15 @@ def __init__(self):
5252
"MPC": {
5353
},
5454
"iLQR": {
55-
"max_iter": 500,
55+
"max_iters": 500,
5656
"init_mu": 1.,
5757
"mu_min": 1e-6,
5858
"mu_max": 1e10,
5959
"init_delta": 2.,
6060
"threshold": 1e-6,
6161
},
6262
"DDP": {
63-
"max_iter": 500,
63+
"max_iters": 500,
6464
"init_mu": 1.,
6565
"mu_min": 1e-6,
6666
"mu_max": 1e10,

‎PythonLinearNonlinearControl/configs/nonlinear_sample_system.py‎

Lines changed: 87 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -6,12 +6,12 @@ class NonlinearSampleSystemConfigModule():
66
ENV_NAME = "NonlinearSampleSystem-v0"
77
PLANNER_TYPE = "Const"
88
TYPE = "Nonlinear"
9-
TASK_HORIZON = 2500
9+
TASK_HORIZON = 2000
1010
PRED_LEN = 10
1111
STATE_SIZE = 2
1212
INPUT_SIZE = 1
1313
DT = 0.01
14-
R = np.diag([0.01])
14+
R = np.diag([1.])
1515
Q = None
1616
Sf = None
1717
# bounds
@@ -46,24 +46,33 @@ def __init__(self):
4646
"noise_sigma": 0.9,
4747
},
4848
"iLQR": {
49-
"max_iter": 500,
49+
"max_iters": 500,
5050
"init_mu": 1.,
5151
"mu_min": 1e-6,
5252
"mu_max": 1e10,
5353
"init_delta": 2.,
5454
"threshold": 1e-6,
5555
},
5656
"DDP": {
57-
"max_iter": 500,
57+
"max_iters": 500,
5858
"init_mu": 1.,
5959
"mu_min": 1e-6,
6060
"mu_max": 1e10,
6161
"init_delta": 2.,
6262
"threshold": 1e-6,
6363
},
64+
"NMPC": {
65+
"threshold": 1e-5,
66+
"max_iters": 1000,
67+
"learning_rate": 0.1
68+
},
6469
"NMPC-CGMRES": {
70+
"threshold": 1e-3
6571
},
6672
"NMPC-Newton": {
73+
"threshold": 1e-3,
74+
"max_iteration": 500,
75+
"learning_rate": 1e-3
6776
},
6877
}
6978

@@ -103,7 +112,7 @@ def state_cost_fn(x, g_x):
103112

104113
return 0.5 * (x[0]**2) + 0.5 * (x[1]**2)
105114

106-
@ staticmethod
115+
@staticmethod
107116
def terminal_state_cost_fn(terminal_x, terminal_g_x):
108117
"""
109118
@@ -123,7 +132,7 @@ def terminal_state_cost_fn(terminal_x, terminal_g_x):
123132

124133
return 0.5 * (terminal_x[0]**2) + 0.5 * (terminal_x[1]**2)
125134

126-
@ staticmethod
135+
@staticmethod
127136
def gradient_cost_fn_with_state(x, g_x, terminal=False):
128137
""" gradient of costs with respect to the state
129138
@@ -147,7 +156,7 @@ def gradient_cost_fn_with_state(x, g_x, terminal=False):
147156

148157
return cost_dx
149158

150-
@ staticmethod
159+
@staticmethod
151160
def gradient_cost_fn_with_input(x, u):
152161
""" gradient of costs with respect to the input
153162
@@ -159,7 +168,7 @@ def gradient_cost_fn_with_input(x, u):
159168
"""
160169
return 2. * u * np.diag(NonlinearSampleSystemConfigModule.R)
161170

162-
@ staticmethod
171+
@staticmethod
163172
def hessian_cost_fn_with_state(x, g_x, terminal=False):
164173
""" hessian costs with respect to the state
165174
@@ -187,7 +196,7 @@ def hessian_cost_fn_with_state(x, g_x, terminal=False):
187196

188197
return hessian[np.newaxis, :, :]
189198

190-
@ staticmethod
199+
@staticmethod
191200
def hessian_cost_fn_with_input(x, u):
192201
""" hessian costs with respect to the input
193202
@@ -202,7 +211,7 @@ def hessian_cost_fn_with_input(x, u):
202211

203212
return np.tile(NonlinearSampleSystemConfigModule.R, (pred_len, 1, 1))
204213

205-
@ staticmethod
214+
@staticmethod
206215
def hessian_cost_fn_with_input_state(x, u):
207216
""" hessian costs with respect to the state and input
208217
@@ -217,3 +226,71 @@ def hessian_cost_fn_with_input_state(x, u):
217226
(pred_len, input_size) = u.shape
218227

219228
return np.zeros((pred_len, input_size, state_size))
229+
230+
@staticmethod
231+
def gradient_hamiltonian_input(x, lam, u, g_x):
232+
"""
233+
234+
Args:
235+
x (numpy.ndarray): shape(pred_len+1, state_size)
236+
lam (numpy.ndarray): shape(pred_len, state_size)
237+
u (numpy.ndarray): shape(pred_len, input_size)
238+
g_xs (numpy.ndarray): shape(pred_len, state_size)
239+
240+
Returns:
241+
F (numpy.ndarray), shape(pred_len, input_size)
242+
"""
243+
if len(x.shape) == 1:
244+
input_size = u.shape[0]
245+
F = np.zeros(input_size)
246+
F[0] = u[0] + lam[1]
247+
248+
return F
249+
250+
elif len(x.shape) == 2:
251+
pred_len, input_size = u.shape
252+
F = np.zeros((pred_len, input_size))
253+
254+
for i in range(pred_len):
255+
F[i, 0] = u[i, 0] + lam[i, 1]
256+
257+
return F
258+
259+
else:
260+
raise NotImplementedError
261+
262+
@staticmethod
263+
def gradient_hamiltonian_state(x, lam, u, g_x):
264+
"""
265+
Args:
266+
x (numpy.ndarray): shape(pred_len+1, state_size)
267+
lam (numpy.ndarray): shape(pred_len, state_size)
268+
u (numpy.ndarray): shape(pred_len, input_size)
269+
g_xs (numpy.ndarray): shape(pred_len, state_size)
270+
271+
Returns:
272+
lam_dot (numpy.ndarray), shape(state_size, )
273+
"""
274+
if len(lam.shape) == 1:
275+
state_size = lam.shape[0]
276+
lam_dot = np.zeros(state_size)
277+
lam_dot[0] = x[0] - (2. * x[0] * x[1] + 1.) * lam[1]
278+
lam_dot[1] = x[1] + lam[0] + \
279+
(-3. * (x[1]**2) - x[0]**2 + 1.) * lam[1]
280+
281+
return lam_dot
282+
283+
elif len(lam.shape) == 2:
284+
pred_len, state_size = lam.shape
285+
lam_dot = np.zeros((pred_len, state_size))
286+
287+
for i in range(pred_len):
288+
lam_dot[i, 0] = x[i, 0] - \
289+
(2. * x[i, 0] * x[i, 1] + 1.) * lam[i, 1]
290+
lam_dot[i, 1] = x[i, 1] + lam[i, 0] + \
291+
(-3. * (x[i, 1]**2) - x[i, 0]**2 + 1.) * lam[i, 1]
292+
293+
return lam_dot
294+
295+
else:
296+
raise NotImplementedError

‎PythonLinearNonlinearControl/configs/two_wheeled.py‎

Lines changed: 96 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,15 @@ class TwoWheeledConfigModule():
2323
Sf = np.diag([5., 5., 1.])
2424
"""
2525
# for track goal
26+
"""
2627
R = np.diag([0.01, 0.01])
2728
Q = np.diag([2.5, 2.5, 0.01])
2829
Sf = np.diag([2.5, 2.5, 0.01])
30+
"""
31+
# for track goal to NMPC
32+
R = np.diag([0.1, 0.1])
33+
Q = np.diag([0.1, 0.1, 0.1])
34+
Sf = np.diag([0.25, 0.25, 0.25])
2935

3036
# bounds
3137
INPUT_LOWER_BOUND = np.array([-1.5, -3.14])
@@ -62,21 +68,26 @@ def __init__(self):
6268
"noise_sigma": 1.,
6369
},
6470
"iLQR": {
65-
"max_iter": 500,
71+
"max_iters": 500,
6672
"init_mu": 1.,
6773
"mu_min": 1e-6,
6874
"mu_max": 1e10,
6975
"init_delta": 2.,
7076
"threshold": 1e-6,
7177
},
7278
"DDP": {
73-
"max_iter": 500,
79+
"max_iters": 500,
7480
"init_mu": 1.,
7581
"mu_min": 1e-6,
7682
"mu_max": 1e10,
7783
"init_delta": 2.,
7884
"threshold": 1e-6,
7985
},
86+
"NMPC": {
87+
"threshold": 1e-3,
88+
"max_iters": 1000,
89+
"learning_rate": 0.1
90+
},
8091
"NMPC-CGMRES": {
8192
},
8293
"NMPC-Newton": {
@@ -232,3 +243,86 @@ def hessian_cost_fn_with_input_state(x, u):
232243
(pred_len, input_size) = u.shape
233244

234245
return np.zeros((pred_len, input_size, state_size))
246+
247+
@staticmethod
248+
def gradient_hamiltonian_input(x, lam, u, g_x):
249+
"""
250+
251+
Args:
252+
x (numpy.ndarray): shape(pred_len+1, state_size)
253+
lam (numpy.ndarray): shape(pred_len, state_size)
254+
u (numpy.ndarray): shape(pred_len, input_size)
255+
g_xs (numpy.ndarray): shape(pred_len, state_size)
256+
257+
Returns:
258+
F (numpy.ndarray), shape(pred_len, input_size)
259+
"""
260+
if len(x.shape) == 1:
261+
input_size = u.shape[0]
262+
F = np.zeros(input_size)
263+
F[0] = u[0] * TwoWheeledConfigModule.R[0, 0] + \
264+
lam[0] * np.cos(x[2]) + lam[1] * np.sin(x[2])
265+
F[1] = u[1] * TwoWheeledConfigModule.R[1, 1] + lam[2]
266+
267+
return F
268+
269+
elif len(x.shape) == 2:
270+
pred_len, input_size = u.shape
271+
F = np.zeros((pred_len, input_size))
272+
273+
for i in range(pred_len):
274+
F[i, 0] = u[i, 0] * TwoWheeledConfigModule.R[0, 0] + \
275+
lam[i, 0] * np.cos(x[i, 2]) + lam[i, 1] * np.sin(x[i, 2])
276+
F[i, 1] = u[i, 1] * TwoWheeledConfigModule.R[1, 1] + lam[i, 2]
277+
278+
return F
279+
else:
280+
raise NotImplementedError
281+
282+
@staticmethod
283+
def gradient_hamiltonian_state(x, lam, u, g_x):
284+
"""
285+
Args:
286+
x (numpy.ndarray): shape(pred_len+1, state_size)
287+
lam (numpy.ndarray): shape(pred_len, state_size)
288+
u (numpy.ndarray): shape(pred_len, input_size)
289+
g_xs (numpy.ndarray): shape(pred_len, state_size)
290+
291+
Returns:
292+
lam_dot (numpy.ndarray), shape(state_size, )
293+
"""
294+
if len(lam.shape) == 1:
295+
state_size = lam.shape[0]
296+
lam_dot = np.zeros(state_size)
297+
lam_dot[0] = \
298+
(x[0] - g_x[0]) * TwoWheeledConfigModule.Q[0, 0]
299+
lam_dot[1] = \
300+
(x[1] - g_x[1]) * TwoWheeledConfigModule.Q[1, 1]
301+
302+
relative_angle = fit_angle_in_range(x[2] - g_x[2])
303+
lam_dot[2] = \
304+
relative_angle * TwoWheeledConfigModule.Q[2, 2] \
305+
- lam[0] * u[0] * np.sin(x[2]) \
306+
+ lam[1] * u[0] * np.cos(x[2])
307+
308+
return lam_dot
309+
310+
elif len(lam.shape) == 2:
311+
pred_len, state_size = lam.shape
312+
lam_dot = np.zeros((pred_len, state_size))
313+
314+
for i in range(pred_len):
315+
lam_dot[i, 0] = \
316+
(x[i, 0] - g_x[i, 0]) * TwoWheeledConfigModule.Q[0, 0]
317+
lam_dot[i, 1] = \
318+
(x[i, 1] - g_x[i, 1]) * TwoWheeledConfigModule.Q[1, 1]
319+
320+
relative_angle = fit_angle_in_range(x[i, 2] - g_x[i, 2])
321+
lam_dot[i, 2] = \
322+
relative_angle * TwoWheeledConfigModule.Q[2, 2] \
323+
- lam[i, 0] * u[i, 0] * np.sin(x[i, 2]) \
324+
+ lam[i, 1] * u[i, 0] * np.cos(x[i, 2])
325+
326+
return lam_dot
327+
else:
328+
raise NotImplementedError

‎PythonLinearNonlinearControl/controllers/ddp.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ def __init__(self, config, model):
4040
config.hessian_cost_fn_with_input_state
4141

4242
# controller parameters
43-
self.max_iter = config.opt_config["DDP"]["max_iter"]
43+
self.max_iters = config.opt_config["DDP"]["max_iters"]
4444
self.init_mu = config.opt_config["DDP"]["init_mu"]
4545
self.mu = self.init_mu
4646
self.mu_min = config.opt_config["DDP"]["mu_min"]
@@ -88,7 +88,7 @@ def obtain_sol(self, curr_x, g_xs):
8888
# line search param
8989
alphas = 1.1**(-np.arange(10)**2)
9090

91-
while opt_count < self.max_iter:
91+
while opt_count < self.max_iters:
9292
accepted_sol = False
9393

9494
# forward

‎PythonLinearNonlinearControl/controllers/ilqr.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ def __init__(self, config, model):
3838
config.hessian_cost_fn_with_input_state
3939

4040
# controller parameters
41-
self.max_iter = config.opt_config["iLQR"]["max_iter"]
41+
self.max_iters = config.opt_config["iLQR"]["max_iters"]
4242
self.init_mu = config.opt_config["iLQR"]["init_mu"]
4343
self.mu = self.init_mu
4444
self.mu_min = config.opt_config["iLQR"]["mu_min"]
@@ -81,7 +81,7 @@ def obtain_sol(self, curr_x, g_xs):
8181
# line search param
8282
alphas = 1.1**(-np.arange(10)**2)
8383

84-
while opt_count < self.max_iter:
84+
while opt_count < self.max_iters:
8585
accepted_sol = False
8686

8787
# forward

‎PythonLinearNonlinearControl/controllers/make_controllers.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
from .mppi_williams import MPPIWilliams
66
from .ilqr import iLQR
77
from .ddp import DDP
8+
from .nmpc import NMPC
89

910

1011
def make_controller(args, config, model):
@@ -23,5 +24,7 @@ def make_controller(args, config, model):
2324
return iLQR(config, model)
2425
elif args.controller_type == "DDP":
2526
return DDP(config, model)
27+
elif args.controller_type == "NMPC":
28+
return NMPC(config, model)
2629

2730
raise ValueError("No controller: {}".format(args.controller_type))

0 commit comments

Comments
 (0)