opts, args = getopt.getopt(sys.argv[1:], 'N:')
for opt, arg in opts:
if opt == '-N':
N = int(arg)
print 'N = %d' % N
Ts = 0.02
Ac, Bc, Cc, Dc = load_model('../data/mecotron.txt')
A, B, CC, DD, _ = cont2discrete([Ac, Bc, Cc, Dc], Ts, method='zoh')
C = np.c_[CC[2, :]].T
D = np.c_[DD[2, :]].T
nx = A.shape[0]
nq = B.shape[1]
ny = C.shape[0]
umin = -0.3
umax = 0.3
rho = 1.e5
Q = rho * C.T.dot(C)
R = np.eye(nq)
controller = rtPGM(A,
B,
C,
D,
Q,
R,
umin,
umax,
N,
terminal_constraint_tol=1e-8)
controller.codegen('src/rtPGM.h', time_analysis=True)
umin = -300.
umax = 300.
nx = A.shape[0]
nq = B.shape[1]
N = 8
# controller
Q = 1e5 * np.eye(nx)
qi = 0.001
R = 1. * np.eye(nq)
controller = rtPGM(A,
B,
C,
D,
Q,
R,
umin,
umax,
N,
integral_fb=True,
integral_weight=qi)
# controller = PGM(A, B, C, D, Q, R, umin, umax, N, integral_fb=True, integral_weight=qi)
# controller = LQR(A, B, C, D, Q, R, integral_fb=True, integral_weight=1e6)
# controller.codegen()
# mpc iterates
n_it = 500
x = np.c_[0.1, 0.].T
r = np.c_[0.].T
x_sv = np.zeros((nx, 0))
q_sv = np.zeros((nq, 0))
# regularization on input rate
CR = np.zeros((N - 1, N))
for k in range(N - 1):
CR[k, k] = 1.
CR[k, k + 1] = -1.
R_extra = 1e10 * CR.T.dot(CR)
gamma_ratio = 0.8 * 0.99
controller = rtPGM(A,
B,
C,
D,
Q,
R,
umin,
umax,
N,
Kf=Kf,
R_extra=R_extra,
gamma_ratio=gamma_ratio,
terminal_constraint=False)
# controller.codegen()
# mpc iterates
n_it = int(20. / Ts)
q = np.zeros((N, 1))
x = np.linalg.inv(S).dot(np.array([[0.05, 0.0, 0.0, 0.]]).T)
r = np.array([[0.0]])
x_sv = np.zeros((nx, 0))
# system: integrator
Ts = 0.01
A = np.array([[1.]])
B = np.array([[Ts]])
C = np.array([[1]])
D = np.array([[0]])
umin = -2.
umax = 2.
nx = A.shape[0]
nq = B.shape[1]
N = 20
# controller
Q = 10000. * np.eye(nx)
R = 1. * np.eye(nq)
controller = rtPGM(A, B, C, D, Q, R, umin, umax, N)
# controller = PGM(A, B, C, D, Q, R, umin, umax, N)
# controller = LQR(A, B, C, D, Q, R)
# mpc iterates
n_it = 500
x = np.c_[0.].T
r = np.c_[0.2].T
x_sv = np.zeros((nx, 0))
q_sv = np.zeros((nq, 0))
xN_sv = np.zeros((1, 0))
VN_sv = np.zeros((1, 0))
for k in range(n_it):
x_sv = np.hstack((x_sv, x))
VN_sv = np.hstack((VN_sv, np.c_[controller.VN(x, r)]))
Q = rho * C1.T.dot(C1)
R = np.eye(nq)
Tfinv = np.vstack((CC[1, :], CC[4, :], CC[0, :], CC[3, :]))
Tf = np.linalg.inv(Tfinv)
g = 9.81
c = 0.1955
l = 0.13
tau = 0.1
n_it = 100
# reference objective
controller = rtPGM(A1,
B1,
C1,
D1,
Q,
R,
umin,
umax,
N,
terminal_constraint_tol=1e-8)
controller.codegen('src/rtPGM.h', time_analysis=True)
nonlinear_codegen(g, c, l, tau, Ts, Ts / 10, Tf, CC.dot(Tf), DD, 'mecotron_nl',
'src/mecotron_nl.h')
lti_codegen(A1, B1, CC, DD, Q, R, [umin], [umax], controller.Tx,
controller.Su.T, Ts, 'mecotron', 'src/mecotron.h')
subprocess.check_output("cd build && make && cd ..", shell=True)
subprocess.check_output(
"./build/mecotron_step_rtpgm -v 1 -t 1 -f %s -n 1 -i %d" %
('mecotron_step_rtpgm.csv', n_it),
shell=True,
stderr=subprocess.STDOUT)
Bc = np.array([[0., 0.], [1. / ms, 0.], [0., -1.], [-1. / mu, 0.]])
Cc = np.array([[-ks / ms, -cs / ms, 0., cs / ms]])
Dc = np.array([[1. / ms, 0.]])
A, B, C, D, _ = cont2discrete([Ac, Bc, Cc, Dc], Ts, method='zoh')
nx = A.shape[0]
nq = 1
ny = C.shape[0]
umin = -1500.
umax = 1500.
N = 200
# controller
Q = C.T.dot(C)
R = np.c_[D[:, 0]].T.dot(np.c_[D[:, 0]])
S = np.c_[D[:, 0]].T.dot(C)
controller = rtPGM(A, np.c_[B[:, 0]], C, np.c_[D[:, 0]], Q, R, umin, umax, N,
S)
# mpc iterates
n_it = 750
q = np.zeros((N, 1))
# dzo = np.zeros((1, n_it))
zo, dzo = get_bump_disturbance(n_it, Ts, 60. * 1.e3 / 3600.)
# zo, dzo = get_road_disturbance(n_it, Ts, 60.*1.e3/3600.)
x = np.c_[0., 0., 0., 0.].T
x_sv = np.zeros((nx, 0))
q_sv = np.zeros((nq, 0))
VN_sv = np.zeros((1, 0))
for k in range(n_it):