def forward_numpy(C, c, x_init, u_lower, u_upper, fc0b):
_C, _c, _x_init, _u_lower, _u_upper, fc0b = [
Variable(torch.Tensor(x).double(), requires_grad=True)
if x is not None else None
for x in [C, c, x_init, u_lower, u_upper, fc0b]
]
dynamics.fcs[0].bias.data[:] = fc0b.data
# dynamics.A.data[:] = fc0b.view(n_state, n_state).data
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=40,
verbose=-1,
exit_unconverged=True,
backprop=False,
max_linesearch_iter=1,
slew_rate_penalty=1.0,
)(_x_init, QuadCost(_C, _c), dynamics)
return util.get_data_maybe(u_lqr.view(-1)).numpy()
def test_lqr_linear_bounded_delta():
npr.seed(1)
n_batch = 2
n_state, n_ctrl, T = 3, 4, 5
n_sc = n_state + n_ctrl
C = npr.randn(T, n_batch, n_sc, n_sc)
C = np.matmul(C.transpose(0, 1, 3, 2), C)
c = npr.randn(T, n_batch, n_sc)
alpha = 0.2
R = np.tile(
np.eye(n_state) + alpha * np.random.randn(n_state, n_state),
(T, n_batch, 1, 1))
S = 0.01 * np.tile(np.random.randn(n_state, n_ctrl), (T, n_batch, 1, 1))
F = np.concatenate((R, S), axis=3)
f = np.tile(npr.randn(n_state), (T, n_batch, 1))
x_init = npr.randn(n_batch, n_state)
u_lower = -npr.random((T, n_batch, n_ctrl))
u_upper = npr.random((T, n_batch, n_ctrl))
tau_cp, objs_cp = lqr_cp(
C[:, 0],
c[:, 0],
F[:, 0],
f[:, 0],
x_init[0],
T,
n_state,
n_ctrl,
u_lower[:, 0],
u_upper[:, 0],
)
tau_cp = tau_cp.T
x_cp = tau_cp[:, :n_state]
u_cp = tau_cp[:, n_state:]
C, c, R, S, F, f, x_init, u_lower, u_upper = [
Variable(torch.Tensor(x).double()) if x is not None else None
for x in [C, c, R, S, F, f, x_init, u_lower, u_upper]
]
dynamics = AffineDynamics(R[0, 0], S[0, 0], f[0, 0])
delta_u = 0.1
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
u_lower,
u_upper,
lqr_iter=1,
verbose=1,
delta_u=delta_u,
backprop=False,
exit_unconverged=False,
)(x_init, QuadCost(C, c), dynamics)
u_lqr = util.get_data_maybe(u_lqr)
assert torch.abs(u_lqr).max() <= delta_u
def test_lqr_linearization():
npr.seed(0)
torch.manual_seed(0)
n_batch, n_state, n_ctrl, T = 2, 3, 4, 5
hidden_sizes = [10]
n_sc = n_state + n_ctrl
C = 10. * npr.randn(T, n_batch, n_sc, n_sc).astype(np.float64)
C = np.matmul(C.transpose(0, 1, 3, 2), C)
c = 10. * npr.randn(T, n_batch, n_sc).astype(np.float64)
x_init = npr.randn(n_batch, n_state).astype(np.float64)
# beta = 0.5
beta = 2.0
u_lower = -beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
u_upper = beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
_C, _c, _x_init, _u_lower, _u_upper = [
Variable(torch.Tensor(x).double(), requires_grad=True)
if x is not None else None for x in [C, c, x_init, u_lower, u_upper]
]
F = Variable(torch.randn(1, 1, n_state, n_sc).repeat(T - 1, 1, 1,
1).double(),
requires_grad=True)
dynamics = NNDynamics(n_state, n_ctrl, hidden_sizes,
activation='sigmoid').double()
u_init = None
_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
grad_method=GradMethods.ANALYTIC,
)
u = torch.randn(T, n_batch, n_ctrl).type_as(_x_init.data)
x = util.get_traj(T, u, x_init=_x_init, dynamics=dynamics)
Fan, fan = _lqr.linearize_dynamics(x, u, dynamics, diff=False)
_lqr.grad_method = GradMethods.AUTO_DIFF
Fau, fau = _lqr.linearize_dynamics(x, u, dynamics, diff=False)
npt.assert_allclose(Fan.data.numpy(), Fau.data.numpy(), atol=1e-4)
npt.assert_allclose(fan.data.numpy(), fau.data.numpy(), atol=1e-4)
# Make sure diff version doesn't crash:
Fau, fau = _lqr.linearize_dynamics(x, u, dynamics, diff=True)
_lqr.grad_method = GradMethods.FINITE_DIFF
Ff, ff = _lqr.linearize_dynamics(x, u, dynamics, diff=False)
npt.assert_allclose(Fan.data.numpy(), Ff.data.numpy(), atol=1e-4)
npt.assert_allclose(fan.data.numpy(), ff.data.numpy(), atol=1e-4)
# Make sure diff version doesn't crash:
Ff, ff = _lqr.linearize_dynamics(x, u, dynamics, diff=True)
def __init__(self, MPDhost='localhost'):
# Init an empty tile
self.emptyTile = pygame.image.load(BACKGROUNDS_DIR +
'blank.png').convert_alpha()
self.tile = self.emptyTile.copy()
pygame.transform.scale(self.emptyTile, TILE_SIZE)
# Load radio graphics - play button, stop button, etc.
self.images = icons.loadRadioImages(RADIO_DIR)
# Station number is 0 through 7.
self.station = 0
# Music playing or stopped
self.playing = False
# Create client connection to MPD
self.mpc = mpc.MPC(MPDhost)
# Volume - 0 to 100 = 0% to 100%
self.volume = 50
self.mpc.setvol(self.volume)
# Empty stations from playlist and build anew
self.mpc.clear()
self.stations = [] #station,url,small image,large image
#self.loadStation('Radio Paradise','http://stream-sd.radioparadise.com:8056','RadioParadise.png')
self.loadStation('Radio Paradise',
'http://stream-dc1.radioparadise.com/mp3-192',
'RadioParadise.png')
#self.loadStation('Ben FM','http://1741.live.streamtheworld.com:80/WCHZFMAAC_SC','BenFM.png')
self.loadStation(
'977 Comedy',
'http://7639.live.streamtheworld.com:80/977_COMEDY_SC',
'977Comedy.png')
self.loadStation(
'Lone Star',
'http://kzps-fm.akacast.akamaistream.net/7/775/20092/v1/auth.akacast.akamaistream.net/kzps-fm',
'LoneStar.png')
# self.loadStation('The Smooth Lounge','http://listen.radionomy.com/the-smooth-lounge','SmoothLounge.png')
self.loadStation('BNR - Coffee House', 'http://173.192.32.199:7160',
'BNR-Coffee House.png')
self.loadStation('TWiT', 'http://twit.am/listen', 'TWiT.png')
self.loadStation(
'Big Classic Hits',
'http://bigclassichits.akacast.akamaistream.net/7/921/61098/v1/auth.akacast.akamaistream.net/bigclassichits',
'BigClassicHits.png')
self.loadStation('Acoustic FM',
'http://listen.radionomy.com/acoustic-fm',
'AcousticFM.png')
self.loadStation('WERU Blue Hill',
'http://stream.weru.org:80/weru-high.mp3', 'WERU.png')
# Create threading variable
self.thread = Thread('') #Empty thread ready for use.
# Force repaint of tile
self.repaint = True
def __init__(self, size, m_col, h_col, mb_col, scroll_col, other_col,
other_highlight_col):
#TFTApp.__init__(self, size)
self.surface = pygame.Surface(size)
self.menu_array = [{
'title': 'controls',
'tracks': FMUApp.controls
}, {
'title': 'streams',
'tracks': FMUApp.streams
}, {
'title': 'archives',
'tracks': FMUApp.archives
}, {
'title': 'other',
'tracks': FMUApp.otherStations
}]
self.app_state = 0
self.menu_state = 0
self.sub_menu_state = 0
self.back_button = False
self.scroll_color = scroll_col
self.menu_color = m_col
self.highlight_color = h_col
self.menu_bg_color = mb_col
self.back_btn_color = other_col
self.back_highlight_color = other_highlight_col
self.menu_height = 98
self.menu_width = 160
self.menu_font_size = 16
self.menu_line_height = 17
self.menu_font = pygame.font.Font('/home/pi/fonts/FUTURA_N.TTF',
self.menu_font_size)
self.menu = pygame.Surface((self.menu_width, self.menu_height))
self.menu_updated = False
self.got_archives = False
#self.screensaver = tftutils.TFTScreensaver((self.menu_width, self.menu_height))
#self.screensaver.fire += self.on_screensaver_fire
self.fmu_scroll = FMUScroll(160, self.scroll_color)
self.mpc = mpc.MPC()
self.mpc.change += self.on_mpc_change
self.url_opener = urllib.FancyURLopener({})
self.exit = tftutils.EventHook()
def forward_numpy(C, c, x_init, u_lower, u_upper, F, f):
_C, _c, _x_init, _u_lower, _u_upper, F, f = [
Variable(torch.Tensor(x).double()) if x is not None else None
for x in [C, c, x_init, u_lower, u_upper, F, f]
]
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=40,
verbose=1,
exit_unconverged=True,
backprop=False,
max_linesearch_iter=2,
)(_x_init, QuadCost(_C, _c), LinDx(F, f))
return util.get_data_maybe(u_lqr.view(-1)).numpy()
def test_memory():
import psutil
torch.manual_seed(0)
n_batch, n_state, n_ctrl, T = 2, 3, 4, 5
n_sc = n_state + n_ctrl
# Randomly initialize a PSD quadratic cost and linear dynamics.
C = torch.randn(T * n_batch, n_sc, n_sc)
C = torch.bmm(C, C.transpose(1, 2)).view(T, n_batch, n_sc, n_sc)
c = torch.randn(T, n_batch, n_sc)
alpha = 0.2
R = (torch.eye(n_state) + alpha * torch.randn(n_state, n_state)).repeat(
T, n_batch, 1, 1)
S = torch.randn(T, n_batch, n_state, n_ctrl)
F = torch.cat((R, S), dim=3)
# The initial state.
x_init = torch.randn(n_batch, n_state)
# The upper and lower control bounds.
u_lower = -torch.rand(T, n_batch, n_ctrl)
u_upper = torch.rand(T, n_batch, n_ctrl)
process = psutil.Process(os.getpid())
# gc.collect()
# start_mem = process.memory_info().rss
# _lqr = LQRStep(
# n_state=n_state,
# n_ctrl=n_ctrl,
# T=T,
# u_lower=u_lower,
# u_upper=u_upper,
# u_zero_I=u_zero_I,
# true_cost=cost,
# true_dynamics=dynamics,
# delta_u=delta_u,
# delta_space=True,
# # current_x=x,
# # current_u=u,
# )
# e = Variable(torch.Tensor())
# x, u = _lqr(x_init, C, c, F, f if f is not None else e)
# gc.collect()
# mem_used = process.memory_info().rss - start_mem
# print(mem_used)
# assert mem_used == 0
gc.collect()
start_mem = process.memory_info().rss
_mpc = mpc.MPC(
n_state=n_state,
n_ctrl=n_ctrl,
T=T,
u_lower=u_lower,
u_upper=u_upper,
lqr_iter=20,
verbose=1,
backprop=False,
exit_unconverged=False,
)
_mpc(x_init, QuadCost(C, c), LinDx(F))
del _mpc
gc.collect()
mem_used = process.memory_info().rss - start_mem
print(mem_used)
assert mem_used == 0
def test_lqr_linear_unbounded():
npr.seed(1)
n_batch = 2
n_state, n_ctrl = 3, 4
n_sc = n_state + n_ctrl
T = 5
C = npr.randn(T, n_batch, n_sc, n_sc)
C = np.matmul(C.transpose(0, 1, 3, 2), C)
c = npr.randn(T, n_batch, n_sc)
alpha = 0.2
R = np.tile(
np.eye(n_state) + alpha * np.random.randn(n_state, n_state),
(T, n_batch, 1, 1))
S = np.tile(np.random.randn(n_state, n_ctrl), (T, n_batch, 1, 1))
F = np.concatenate((R, S), axis=3)
f = np.tile(npr.randn(n_state), (T, n_batch, 1))
x_init = npr.randn(n_batch, n_state)
# u_lower = -100.*npr.random((T, n_batch, n_ctrl))
# u_upper = 100.*npr.random((T, n_batch, n_ctrl))
u_lower = -1e4 * np.ones((T, n_batch, n_ctrl))
u_upper = 1e4 * np.ones((T, n_batch, n_ctrl))
tau_cp, objs_cp = lqr_cp(C[:, 0], c[:, 0], F[:, 0], f[:, 0], x_init[0], T,
n_state, n_ctrl, None, None)
tau_cp = tau_cp.T
x_cp = tau_cp[:, :n_state]
u_cp = tau_cp[:, n_state:]
C, c, R, S, F, f, x_init, u_lower, u_upper = [
Variable(torch.Tensor(x).double()) if x is not None else None
for x in [C, c, R, S, F, f, x_init, u_lower, u_upper]
]
dynamics = AffineDynamics(R[0, 0], S[0, 0], f[0, 0])
u_lqr = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
u_lower,
u_upper,
u_lqr,
lqr_iter=10,
backprop=False,
verbose=1,
exit_unconverged=True,
)(x_init, QuadCost(C, c), dynamics)
tau_lqr = torch.cat((x_lqr, u_lqr), 2)
tau_lqr = util.get_data_maybe(tau_lqr)
npt.assert_allclose(tau_cp, tau_lqr[:, 0].numpy(), rtol=1e-3)
u_lqr = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
None,
None,
u_lqr,
lqr_iter=10,
backprop=False,
exit_unconverged=False,
)(x_init, QuadCost(C, c), dynamics)
tau_lqr = torch.cat((x_lqr, u_lqr), 2)
tau_lqr = util.get_data_maybe(tau_lqr)
npt.assert_allclose(tau_cp, tau_lqr[:, 0].numpy(), rtol=1e-3)
def test_lqr_slew_rate():
n_batch = 2
n_state, n_ctrl = 3, 4
n_sc = n_state + n_ctrl
T = 5
alpha = 0.2
torch.manual_seed(1)
C = torch.randn(T, n_batch, n_sc, n_sc)
C = C.transpose(2, 3).matmul(C)
c = torch.randn(T, n_batch, n_sc)
x_init = torch.randn(n_batch, n_state)
R = torch.eye(n_state) + alpha * torch.randn(n_state, n_state)
S = torch.randn(n_state, n_ctrl)
f = torch.randn(n_state)
C, c, x_init, R, S, f = map(Variable, (C, c, x_init, R, S, f))
dynamics = AffineDynamics(R, S, f)
x, u, objs = mpc.MPC(
n_state,
n_ctrl,
T,
u_lower=None,
u_upper=None,
u_init=None,
lqr_iter=10,
backprop=False,
verbose=1,
exit_unconverged=False,
eps=1e-4,
)(x_init, QuadCost(C, c), dynamics)
# The solution should be the same when the slew rate approaches 0.
x_slew_eps, u_slew_eps, objs_slew_eps = mpc.MPC(
n_state,
n_ctrl,
T,
u_lower=None,
u_upper=None,
u_init=None,
lqr_iter=10,
backprop=False,
verbose=1,
exit_unconverged=False,
eps=1e-4,
slew_rate_penalty=1e-6,
)(x_init, QuadCost(C, c), dynamics)
npt.assert_allclose(x.data.numpy(), x_slew_eps.data.numpy(), atol=1e-3)
npt.assert_allclose(u.data.numpy(), u_slew_eps.data.numpy(), atol=1e-3)
x_slew, u_slew, objs_slew = mpc.MPC(
n_state,
n_ctrl,
T,
u_lower=None,
u_upper=None,
u_init=None,
lqr_iter=10,
backprop=False,
verbose=1,
exit_unconverged=False,
eps=1e-4,
slew_rate_penalty=1.,
)(x_init, QuadCost(C, c), dynamics)
assert np.alltrue((objs < objs_slew).numpy())
d = torch.norm(u[:-1] - u[1:]).item()
d_slew = torch.norm(u_slew[:-1] - u_slew[1:]).item()
assert d_slew < d
def test_lqr_backward_cost_nn_dynamics_module_constrained_slew():
npr.seed(0)
torch.manual_seed(0)
n_batch, n_state, n_ctrl, T = 1, 2, 2, 2
hidden_sizes = [10, 10]
n_sc = n_state + n_ctrl
C = 10. * npr.randn(T, n_batch, n_sc, n_sc).astype(np.float64)
C = np.matmul(C.transpose(0, 1, 3, 2), C)
c = 10. * npr.randn(T, n_batch, n_sc).astype(np.float64)
x_init = npr.randn(n_batch, n_state).astype(np.float64)
beta = 1.
u_lower = -beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
u_upper = beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
dynamics = NNDynamics(n_state, n_ctrl, hidden_sizes,
activation='sigmoid').double()
fc0b = dynamics.fcs[0].bias.view(-1).data.numpy().copy()
def forward_numpy(C, c, x_init, u_lower, u_upper, fc0b):
_C, _c, _x_init, _u_lower, _u_upper, fc0b = [
Variable(torch.Tensor(x).double(), requires_grad=True)
if x is not None else None
for x in [C, c, x_init, u_lower, u_upper, fc0b]
]
dynamics.fcs[0].bias.data[:] = fc0b.data
# dynamics.A.data[:] = fc0b.view(n_state, n_state).data
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=40,
verbose=-1,
exit_unconverged=True,
backprop=False,
max_linesearch_iter=1,
slew_rate_penalty=1.0,
)(_x_init, QuadCost(_C, _c), dynamics)
return util.get_data_maybe(u_lqr.view(-1)).numpy()
def f_c(c_flat):
c_ = c_flat.reshape(T, n_batch, n_sc)
return forward_numpy(C, c_, x_init, u_lower, u_upper, fc0b)
def f_fc0b(fc0b):
return forward_numpy(C, c, x_init, u_lower, u_upper, fc0b)
u = forward_numpy(C, c, x_init, u_lower, u_upper, fc0b)
# Make sure the solution is strictly partially on the boundary.
assert np.any(u == u_lower.reshape(-1)) or np.any(u == u_upper.reshape(-1))
assert np.any((u != u_lower.reshape(-1)) & (u != u_upper.reshape(-1)))
du_dc_fd = nd.Jacobian(f_c)(c.reshape(-1))
du_dfc0b_fd = nd.Jacobian(f_fc0b)(fc0b.reshape(-1))
dynamics.fcs[0].bias.data = torch.DoubleTensor(fc0b).clone()
_C, _c, _x_init, _u_lower, _u_upper, fc0b = [
Variable(torch.Tensor(x).double(), requires_grad=True)
if x is not None else None
for x in [C, c, x_init, u_lower, u_upper, fc0b]
]
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=20,
verbose=-1,
max_linesearch_iter=1,
grad_method=GradMethods.ANALYTIC,
slew_rate_penalty=1.0,
)(_x_init, QuadCost(_C, _c), dynamics)
u_lqr_flat = u_lqr.view(-1)
du_dC = []
du_dc = []
du_dfc0b = []
for i in range(len(u_lqr_flat)):
dCi = grad(u_lqr_flat[i], [_C],
retain_graph=True)[0].contiguous().view(-1)
dci = grad(u_lqr_flat[i], [_c],
retain_graph=True)[0].contiguous().view(-1)
dfc0b = grad(u_lqr_flat[i], [dynamics.fcs[0].bias],
retain_graph=True)[0].view(-1)
du_dC.append(dCi)
du_dc.append(dci)
du_dfc0b.append(dfc0b)
du_dC = torch.stack(du_dC).data.numpy()
du_dc = torch.stack(du_dc).data.numpy()
du_dfc0b = torch.stack(du_dfc0b).data.numpy()
npt.assert_allclose(du_dc_fd, du_dc, atol=1e-3)
npt.assert_allclose(du_dfc0b_fd, du_dfc0b, atol=1e-3)
def test_lqr_backward_cost_affine_dynamics_module_constrained():
npr.seed(0)
torch.manual_seed(0)
n_batch, n_state, n_ctrl, T = 1, 2, 2, 2
hidden_sizes = [10]
n_sc = n_state + n_ctrl
C = 10. * npr.randn(T, n_batch, n_sc, n_sc).astype(np.float64)
C = np.matmul(C.transpose(0, 1, 3, 2), C)
c = 10. * npr.randn(T, n_batch, n_sc).astype(np.float64)
x_init = npr.randn(n_batch, n_state).astype(np.float64)
# beta = 0.5
beta = 2.0
u_lower = -beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
u_upper = beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
_C, _c, _x_init, _u_lower, _u_upper = [
Variable(torch.Tensor(x).double(), requires_grad=True)
if x is not None else None for x in [C, c, x_init, u_lower, u_upper]
]
F = Variable(torch.randn(1, 1, n_state, n_sc).repeat(T - 1, 1, 1,
1).double(),
requires_grad=True)
dynamics = AffineDynamics(F[0, 0, :, :n_state], F[0, 0, :, n_state:])
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=20,
verbose=1,
)(_x_init, QuadCost(_C, _c), LinDx(F))
u_lqr_flat = u_lqr.view(-1)
du_dF = []
for i in range(len(u_lqr_flat)):
dF = grad(u_lqr_flat[i], [F], retain_graph=True)[0].view(-1)
du_dF.append(dF)
du_dF = torch.stack(du_dF).data.numpy()
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=20,
verbose=1,
)(_x_init, QuadCost(_C, _c), dynamics)
u_lqr_flat = u_lqr.view(-1)
du_dF_ = []
for i in range(len(u_lqr_flat)):
dF = grad(u_lqr_flat[i], [F], retain_graph=True)[0].view(-1)
du_dF_.append(dF)
du_dF_ = torch.stack(du_dF_).data.numpy()
npt.assert_allclose(du_dF, du_dF_, atol=1e-4)
def test_lqr_backward_cost_linear_dynamics_constrained():
npr.seed(0)
torch.manual_seed(0)
n_batch, n_state, n_ctrl, T = 1, 2, 2, 3
hidden_sizes = [10, 10]
n_sc = n_state + n_ctrl
C = 10. * npr.randn(T, n_batch, n_sc, n_sc).astype(np.float64)
C = np.matmul(C.transpose(0, 1, 3, 2), C)
c = 10. * npr.randn(T, n_batch, n_sc).astype(np.float64)
x_init = npr.randn(n_batch, n_state).astype(np.float64)
beta = 0.5
u_lower = -beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
u_upper = beta * np.ones((T, n_batch, n_ctrl)).astype(np.float64)
F = npr.randn(T - 1, n_batch, n_state, n_sc)
f = npr.randn(T - 1, n_batch, n_state)
def forward_numpy(C, c, x_init, u_lower, u_upper, F, f):
_C, _c, _x_init, _u_lower, _u_upper, F, f = [
Variable(torch.Tensor(x).double()) if x is not None else None
for x in [C, c, x_init, u_lower, u_upper, F, f]
]
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=40,
verbose=1,
exit_unconverged=True,
backprop=False,
max_linesearch_iter=2,
)(_x_init, QuadCost(_C, _c), LinDx(F, f))
return util.get_data_maybe(u_lqr.view(-1)).numpy()
def f_c(c_flat):
c_ = c_flat.reshape(T, n_batch, n_sc)
return forward_numpy(C, c_, x_init, u_lower, u_upper, F, f)
def f_F(F_flat):
F_ = F_flat.reshape(T - 1, n_batch, n_state, n_sc)
return forward_numpy(C, c, x_init, u_lower, u_upper, F_, f)
def f_f(f_flat):
f_ = f_flat.reshape(T - 1, n_batch, n_state)
return forward_numpy(C, c, x_init, u_lower, u_upper, F, f_)
def f_x_init(x_init):
x_init = x_init.reshape(1, -1)
return forward_numpy(C, c, x_init, u_lower, u_upper, F, f)
u = forward_numpy(C, c, x_init, u_lower, u_upper, F, f)
# Make sure the solution is strictly partially on the boundary.
assert np.any(u == u_lower.reshape(-1)) or np.any(u == u_upper.reshape(-1))
assert np.any((u != u_lower.reshape(-1)) & (u != u_upper.reshape(-1)))
du_dc_fd = nd.Jacobian(f_c)(c.reshape(-1))
du_dF_fd = nd.Jacobian(f_F)(F.reshape(-1))
du_df_fd = nd.Jacobian(f_f)(f.reshape(-1))
du_dxinit_fd = nd.Jacobian(f_x_init)(x_init[0])
_C, _c, _x_init, _u_lower, _u_upper, F, f = [
Variable(torch.Tensor(x).double(), requires_grad=True)
if x is not None else None
for x in [C, c, x_init, u_lower, u_upper, F, f]
]
u_init = None
x_lqr, u_lqr, objs_lqr = mpc.MPC(
n_state,
n_ctrl,
T,
_u_lower,
_u_upper,
u_init,
lqr_iter=20,
verbose=1,
)(_x_init, QuadCost(_C, _c), LinDx(F, f))
u_lqr_flat = u_lqr.view(-1)
du_dC = []
du_dc = []
du_dF = []
du_df = []
du_dx_init = []
for i in range(len(u_lqr_flat)):
dCi = grad(u_lqr_flat[i], [_C], retain_graph=True)[0].view(-1)
dci = grad(u_lqr_flat[i], [_c], retain_graph=True)[0].view(-1)
dF = grad(u_lqr_flat[i], [F], retain_graph=True)[0].view(-1)
df = grad(u_lqr_flat[i], [f], retain_graph=True)[0].view(-1)
dx_init = grad(u_lqr_flat[i], [_x_init], retain_graph=True)[0].view(-1)
du_dC.append(dCi)
du_dc.append(dci)
du_dF.append(dF)
du_df.append(df)
du_dx_init.append(dx_init)
du_dC = torch.stack(du_dC).data.numpy()
du_dc = torch.stack(du_dc).data.numpy()
du_dF = torch.stack(du_dF).data.numpy()
du_df = torch.stack(du_df).data.numpy()
du_dx_init = torch.stack(du_dx_init).data.numpy()
npt.assert_allclose(du_dc_fd, du_dc, atol=1e-4)
npt.assert_allclose(du_dF, du_dF_fd, atol=1e-4)
npt.assert_allclose(du_df, du_df_fd, atol=1e-4)
npt.assert_allclose(du_dx_init, du_dxinit_fd, atol=1e-4)
index_nowref = temp
waypoints_nc = []
Uf_nc = []
for i in range(Np):
if index_nowref + i <= len_waypoints - 1:
waypoints_nc.append(waypoints[index_nowref + i])
else:
waypoints_nc.append(waypoints[len_waypoints - 1])
Uf_nc.append(mpc.U(waypoints_nc[i][3], waypoints_nc[i][4]))
Xf = mpc.X(waypoints[index_nowref][0], waypoints[index_nowref][1],
waypoints[index_nowref][2])
Uf = mpc.U(waypoints[index_nowref][3], waypoints[index_nowref][4])
a_max = mpc.U(2, math.pi / 6)
Umax = mpc.U(1.3, math.pi / 4)
v, delta = mpc.MPC(X, Xf, Uf, Uk_1, Ufk_1, Uf_nc, 0.02, Np, a_max, Umax,
2.95)
plt.plot(waypoints[index_nowref][0],
waypoints[index_nowref][1],
color="red",
marker=".")
log.write(
"----------------------------------------------------------------------------------------------------------\n"
)
log.write("时刻:{:<5f} 参考点:{}\n".format(t, index_nowref))
log.write(
"当前状态: x={:<15f} y={:<15f} heading={:<15f} 计算控制量: v={:<15f} delta={:<15f}\n"
.format(X.x, X.y, X.theta, v, delta))
log.write(
"参考状态: x={:<15f} y={:<15f} heading={:<15f} 参考控制量: v={:<15f} delta={:<15f}\n"
.format(Xf.x, Xf.y, Xf.theta, Uf.v, Uf.delta))
Uk_1 = mpc.U(v, delta)
plt.plot(x_ref, y_ref)
plt.plot(x_model_estimated, y_model_estimated, 'red')
plt.subplot(222)
plt.plot(yaw_ref)
plt.plot(yaw_model_estimated, 'red')
plt.title('Yaw')
plt.subplot(223)
plt.plot(x_acc_veh)
plt.title('XAcc')
plt.subplot(224)
plt.plot(np.rad2deg(steer))
plt.title('Steer')
plt.show()
exit()
mpc = mpc.MPC()
x_mpc = []
y_mpc = []
yaw_mpc = []
speed_mpc = []
steer_mpc = []
a_mpc = []
time_mpc = []
acc_control = 0
steer_control = 0
start_index = 0
end_index = len(x_ref) - 50
print('Lenght: ' + str(len(x_ref)))
def backward(self, dl_dx, dl_du):
start = time.time()
x_init, C, c, F, f, new_x, new_u = self.saved_tensors
r = []
for t in range(self.T):
rt = torch.cat((dl_dx[t], dl_du[t]), 1)
r.append(rt)
r = torch.stack(r)
if self.u_lower is None:
I = None
else:
I = (torch.abs(new_u - self.u_lower) <= 1e-8) | \
(torch.abs(new_u - self.u_upper) <= 1e-8)
dx_init = Variable(torch.zeros_like(x_init))
_mpc = mpc.MPC(
self.n_state,
self.n_ctrl,
self.T,
u_zero_I=I,
u_init=None,
lqr_iter=1,
verbose=-1,
n_batch=C.size(1),
delta_u=None,
# exit_unconverged=True, # It's really bad if this doesn't converge.
exit_unconverged=False, # It's really bad if this doesn't converge.
eps=self.back_eps,
)
dx, du, _ = _mpc(dx_init, mpc.QuadCost(C, -r), mpc.LinDx(F, None))
dx, du = dx.data, du.data
dxu = torch.cat((dx, du), 2)
xu = torch.cat((new_x, new_u), 2)
dC = torch.zeros_like(C)
for t in range(self.T):
xut = torch.cat((new_x[t], new_u[t]), 1)
dxut = dxu[t]
dCt = -0.5 * (util.bger(dxut, xut) + util.bger(xut, dxut))
dC[t] = dCt
dc = -dxu
lams = []
prev_lam = None
for t in range(self.T - 1, -1, -1):
Ct_xx = C[t, :, :self.n_state, :self.n_state]
Ct_xu = C[t, :, :self.n_state, self.n_state:]
ct_x = c[t, :, :self.n_state]
xt = new_x[t]
ut = new_u[t]
lamt = util.bmv(Ct_xx, xt) + util.bmv(Ct_xu, ut) + ct_x
if prev_lam is not None:
Fxt = F[t, :, :, :self.n_state].transpose(1, 2)
lamt += util.bmv(Fxt, prev_lam)
lams.append(lamt)
prev_lam = lamt
lams = list(reversed(lams))
dlams = []
prev_dlam = None
for t in range(self.T - 1, -1, -1):
dCt_xx = C[t, :, :self.n_state, :self.n_state]
dCt_xu = C[t, :, :self.n_state, self.n_state:]
drt_x = -r[t, :, :self.n_state]
dxt = dx[t]
dut = du[t]
dlamt = util.bmv(dCt_xx, dxt) + util.bmv(dCt_xu, dut) + drt_x
if prev_dlam is not None:
Fxt = F[t, :, :, :self.n_state].transpose(1, 2)
dlamt += util.bmv(Fxt, prev_dlam)
dlams.append(dlamt)
prev_dlam = dlamt
dlams = torch.stack(list(reversed(dlams)))
dF = torch.zeros_like(F)
for t in range(self.T - 1):
xut = xu[t]
lamt = lams[t + 1]
dxut = dxu[t]
dlamt = dlams[t + 1]
dF[t] = -(util.bger(dlamt, xut) + util.bger(lamt, dxut))
if f.nelement() > 0:
_dlams = dlams[1:]
assert _dlams.shape == f.shape
df = -_dlams
else:
df = torch.Tensor()
dx_init = -dlams[0]
self.backward_time = time.time() - start
return dx_init, dC, dc, dF, df