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