raiot
/
PythonLinearNonlinearControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix bug of nmpc newton

This commit is contained in:
Shunichi09 2019-10-09 14:01:01 +09:00
parent ac1f2d2ebd
commit 37a7d720f6
2 changed files with 91 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
nmpc/cgmres/main_two_wheeled.py
View File

@ -60,7 +60,7 @@ class TwoWheeledSystem():
k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2) k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2)
for i, func in enumerate(functions): for i, func in enumerate(functions):
k2[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2) k2[i] = dt * func(self.x_1 + k1[0]/2., self.x_2 + k1[1]/2., self.x_3 + k1[2]/2., u_1, u_2)
for i, func in enumerate(functions): for i, func in enumerate(functions):
k3[i] = dt * func(self.x_1 + k2[0], self.x_2 + k2[1], self.x_3 + k2[2], u_1, u_2) k3[i] = dt * func(self.x_1 + k2[0], self.x_2 + k2[1], self.x_3 + k2[2], u_1, u_2)

146
nmpc/newton/main_example.py
View File

@ -1,6 +1,7 @@
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import math import math
import copy
class SampleSystem(): class SampleSystem():
"""SampleSystem, this is the simulator """SampleSystem, this is the simulator
@ -63,8 +64,8 @@ class SampleSystem():
self.x_2 += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. self.x_2 += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6.
# save # save
self.history_x_1.append(self.x_1) self.history_x_1.append(copy.deepcopy(self.x_1))
self.history_x_2.append(self.x_2) self.history_x_2.append(copy.deepcopy(self.x_2))
def _func_x_1(self, y_1, y_2, u): def _func_x_1(self, y_1, y_2, u):
""" """
@ -161,13 +162,17 @@ class NMPCSimulatorSystem():
predicted x_2s for N steps predicted x_2s for N steps
""" """
# initial state # initial state
x_1s = [x_1] x_1s = np.zeros(N+1)
x_2s = [x_2] x_2s = np.zeros(N+1)
# input initial state
x_1s[0] = x_1
x_2s[0] = x_2
for i in range(N): for i in range(N):
temp_x_1, temp_x_2 = self._predict_state_with_oylar(x_1s[i], x_2s[i], us[i], dt) temp_x_1, temp_x_2 = self._predict_state_with_oylar(x_1s[i], x_2s[i], us[i], dt)
x_1s.append(temp_x_1) x_1s[i+1] = temp_x_1
x_2s.append(temp_x_2) x_2s[i+1] = temp_x_2
return x_1s, x_2s return x_1s, x_2s
@ -195,13 +200,17 @@ class NMPCSimulatorSystem():
""" """
# final state # final state
# final_state_func # final_state_func
lam_1s = [x_1s[-1]] lam_1s = np.zeros(N)
lam_2s = [x_2s[-1]] lam_2s = np.zeros(N)
# input final state
lam_1s[-1] = x_1s[-1]
lam_2s[-1] = x_2s[-1]
for i in range(N-1, 0, -1): for i in range(N-1, 0, -1):
temp_lam_1, temp_lam_2 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], lam_1s[0] ,lam_2s[0], us[i], dt) temp_lam_1, temp_lam_2 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], lam_1s[i] ,lam_2s[i], us[i], dt)
lam_1s.insert(0, temp_lam_1) lam_1s[i-1] = temp_lam_1
lam_2s.insert(0, temp_lam_2) lam_2s[i-1] = temp_lam_2
return lam_1s, lam_2s return lam_1s, lam_2s
@ -357,49 +366,60 @@ class NMPCSimulatorSystem():
return y_dot return y_dot
def calc_numerical_gradient(f, x, shape): def calc_numerical_gradient(forward_prop, grad_f, all_us, shape, input_size):
""" """
Parameters Parameters
------------ ------------
f : function forward_prop : function
forward function of NN forward prop
x : numpy.ndarray grad_f : function
input gradient function
all_us : numpy.ndarray, shape(pred_len, input_size*3)
all inputs including with dummy input
shape : tuple shape : tuple
jacobian shape shape of Jacobian
input_size : int
input size of system
Returns Returns
--------- ---------
grad : numpy.ndarray, shape is the same as shape grad : numpy.ndarray, shape is the same as shape
results of numercial gradient of the input results of numercial gradient of the input
References References
----------- -----------
- oreilly japan 0 から作るdeeplearning - oreilly japan 0 から作るdeeplearning
https://github.com/oreilly-japan/deep-learning-from-scratch/blob/master/common/gradient.py https://github.com/oreilly-japan/deep-learning-from-scratch/blob/master/common/gradient.py
""" """
# check condition
if not callable(f):
raise TypeError("f should be callable")
if not (isinstance(x, list) or isinstance(x, np.ndarray)):
raise TypeError("x should be array-like")
h = 1e-3 # 0.01 h = 1e-3 # 0.01
grad = np.zeros(shape) grad = np.zeros(shape)
grad_idx = 0
for idx in range(x.size): it = np.nditer(all_us, flags=['multi_index'], op_flags=['readwrite'])
# save while not it.finished:
tmp_val = x[idx] # get index
idx = it.multi_index
# save and return
tmp_val = all_us[idx]
x[idx] = float(tmp_val) + h # 差分を取る
fxh1 = f(x) # f(x+h) # 上側の差分
all_us[idx] = float(tmp_val) + h
us = all_us[:, :input_size]
x_1s, x_2s, lam_1s, lam_2s = forward_prop(us) # forward
fuh1 = grad_f(x_1s, x_2s, lam_1s, lam_2s, all_us)
x[idx] = float(tmp_val) - h # 下側の差分
fxh2 = f(x) # f(x-h) all_us[idx] = float(tmp_val) - h
us = all_us[:, :input_size]
x_1s, x_2s, lam_1s, lam_2s = forward_prop(us) # forward
fuh2 = grad_f(x_1s, x_2s, lam_1s, lam_2s, all_us)
grad[:, idx] = (fxh1 - fxh2) / (2*h) grad[:, grad_idx] = ((fuh1 - fuh2) / (2.*h)).flatten() # to flat でgradに代入できるように
x[idx] = tmp_val all_us[idx] = tmp_val
it.iternext()
grad_idx += 1
return np.array(grad) return np.array(grad)
@ -441,8 +461,9 @@ class NMPCControllerWithNewton():
self.N = 10 # time step self.N = 10 # time step
self.threshold = 0.0001 # break self.threshold = 0.0001 # break
self.NUM_INPUT = 3 # u with dummy, and 制約条件に対するrawにも合わせた入力の数 self.NUM_ALL_INPUT = 3 # u with dummy, and 制約条件に対するrawにも合わせた入力の数
self.Jacobian_size = self.NUM_INPUT * self.N self.NUM_INPUT = 1 # u with dummy, and 制約条件に対するrawにも合わせた入力の数
self.Jacobian_size = self.NUM_ALL_INPUT * self.N
# newton parameters # newton parameters
self.MAX_ITERATION = 100 self.MAX_ITERATION = 100
@ -451,9 +472,9 @@ class NMPCControllerWithNewton():
self.simulator = NMPCSimulatorSystem() self.simulator = NMPCSimulatorSystem()
# initial # initial
self.us = np.zeros(self.N) self.us = np.zeros((self.N, self.NUM_INPUT))
self.dummy_us = np.ones(self.N) * 0.25 self.dummy_us = np.ones((self.N, self.NUM_INPUT)) * 0.25
self.raws = np.ones(self.N) * 0.01 self.raws = np.ones((self.N, self.NUM_INPUT)) * 0.01
# for fig # for fig
self.history_u = [] self.history_u = []
@ -479,9 +500,8 @@ class NMPCControllerWithNewton():
# calculating sampling time # calculating sampling time
dt = 0.01 dt = 0.01
# concat all us, shape (NUM_INPUT, N) # concat all us, shape (pred_len, input_size)
all_us = np.stack((self.us, self.dummy_us, self.raws)) all_us = np.hstack((self.us, self.dummy_us, self.raws))
all_us = all_us.T.flatten()
# Newton method # Newton method
for i in range(self.MAX_ITERATION): for i in range(self.MAX_ITERATION):
@ -496,22 +516,30 @@ class NMPCControllerWithNewton():
# print("break!!") # print("break!!")
break break
grad_f = lambda all_us : self._calc_f(x_1s, x_2s, lam_1s, lam_2s, all_us, self.N, dt) grad_f = lambda x_1s, x_2s, lam_1s, lam_2s, all_us : self._calc_f(x_1s, x_2s, lam_1s, lam_2s, all_us, self.N, dt)
grads = calc_numerical_gradient(grad_f, all_us, (self.Jacobian_size, self.Jacobian_size)) forward_prop_f = lambda us : self.simulator.calc_predict_and_adjoint_state(x_1, x_2, us, self.N, dt)
grads = calc_numerical_gradient(forward_prop_f, grad_f, all_us, (self.Jacobian_size, self.Jacobian_size), self.NUM_INPUT)
# make jacobian and calc inverse of it # make jacobian and calc inverse of it
# all us # grads += np.eye(self.Jacobian_size) * 1e-8
all_us = all_us - np.dot(np.linalg.inv(grads), F_hat) try:
all_us = all_us.reshape(-1, 1) - np.dot(np.linalg.inv(grads), F_hat.reshape(-1, 1))
except np.linalg.LinAlgError:
print("Warning : singular matrix!!")
grads += np.eye(self.Jacobian_size) * 1e-10 # add noise
all_us = all_us.reshape(-1, 1) - np.dot(np.linalg.inv(grads), F_hat.reshape(-1, 1))
all_us = all_us.reshape(self.N, self.NUM_ALL_INPUT)
# update # update
self.us = all_us[::self.NUM_INPUT] self.us = all_us[:, :self.NUM_INPUT]
self.dummy_us = all_us[1::self.NUM_INPUT] self.dummy_us = all_us[:, self.NUM_INPUT:2*self.NUM_INPUT]
self.raws = all_us[2::self.NUM_INPUT] self.raws = all_us[:, 2*self.NUM_INPUT:]
# final insert # final insert
self.us = all_us[::self.NUM_INPUT] self.us = all_us[:, :self.NUM_INPUT]
self.dummy_us = all_us[1::self.NUM_INPUT] self.dummy_us = all_us[:, self.NUM_INPUT:2*self.NUM_INPUT]
self.raws = all_us[2::self.NUM_INPUT] self.raws = all_us[:, 2*self.NUM_INPUT:]
x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt)
@ -548,16 +576,18 @@ class NMPCControllerWithNewton():
dt : float dt : float
sampling time of system sampling time of system
""" """
F = [] F = np.zeros((N, self.NUM_INPUT*3))
us = all_us[::self.NUM_INPUT] us = all_us[:, :self.NUM_INPUT].flatten()
dummy_us = all_us[1::self.NUM_INPUT] dummy_us = all_us[:, self.NUM_INPUT:2*self.NUM_INPUT].flatten()
raws = all_us[2::self.NUM_INPUT] raws = all_us[:, 2*self.NUM_INPUT:].flatten()
for i in range(N): for i in range(N):
F.append(0.5 * us[i] + lam_2s[i] + 2. * raws[i] * us[i]) F_u = 0.5 * us[i] + lam_2s[i] + 2. * raws[i] * us[i]
F.append(-0.01 + 2. * raws[i] * dummy_us[i]) F_dummy = -0.01 + 2. * raws[i] * dummy_us[i]
F.append(us[i]**2 + dummy_us[i]**2 - 0.5**2) F_raw = us[i]**2 + dummy_us[i]**2 - 0.5**2
F[i] = np.array([F_u, F_dummy, F_raw])
return np.array(F) return np.array(F)