class System(object): """ a collection of callables, mostly """ def __init__(self, func, tlims=(0, 10), **kwargs): # TODO add an option to pass a symSystem directly # skipping a bunch of middleman bullshit that might # otherwise have to happen self.tlims = tlims keys = kwargs.keys() if 'xinit' in keys: self.xinit = kwargs['xinit'] self.dim = len(self.xinit) elif 'dim' in keys: self.dim = kwargs['dim'] else: self.dim = 1 self.f = func self.ufun = kwargs['controller'] if 'controller' in keys else None self.dfdx = kwargs['dfdx'] if 'dfdx' in keys else None self.dfdu = kwargs['dfdu'] if 'dfdu' in keys else None self.phi = kwargs['phi'] if 'phi' in keys else None self.delf = kwargs['delf'] if 'delf' in kwargs else None if self.ufun is None: self.dimu = 0 else: self.dimu = len(self.dimu(tlims[0])) # to be called after a reference has been set def build_cost(self, **kwargs): self.cost = CostFunction(self.dim, self.dimu, self.ref, projector=self.project, **kwargs) return self.cost def set_u(self, controller): if 'ufun' in self.__dict__.keys(): self._uold = self.ufun self.ufun = controller def reset_u(self): if '_uold' in self.__dict__.keys(): self.u = self._uold else: print("Nothing to reset to. Not doing anything.") # TODO make sure this works in all combinations of linearization # or not, and controlled or not; # Major cleanup needed. def integrate(self, use_jac=False, linearize=True, interpolate=True, **kwargs): keys = kwargs.keys() xinit = kwargs['xinit'] if 'xinit' in keys else self.xinit lin = linearize interp = interpolate if self.ufun is not None: func = lambda t, x: self.f(t, x, self.ufun(t, x)) dfdx = lambda t, x: self.dfdx(t, x, self.ufun(t, x)) dfdu = lambda t, x: self.dfdu(t, x, self.ufun(t, x)) else: func = self.f dfdx = self.dfdx dfdu = self.dfdu jac = dfdx if use_jac else None #Tracer()() if self.delf is not None: delfunc = lambda t, x: self.delf(t, x, self.ufun(t, x)) (t, x, jumps) = sysIntegrate(func, self.xinit, tlims=self.tlims, phi=self.phi, jac=jac, delfunc=delfunc) else: (t, x, jumps) = sysIntegrate(func, self.xinit, tlims=self.tlims, phi=self.phi, jac=jac) #Tracer()() components = ['x'] if 'ufun' in self.__dict__.keys(): components.append('u') if lin: components.append('A') components.append('B') traj = Trajectory(*components) for (tt, xx) in zip(t, x): names = {'x': xx} if 'u' in components: names['u'] = self.ufun(tt, xx) if 'A' in components: names['A'] = dfdx(tt, xx) if 'B' in components: names['B'] = dfdu(tt, xx) traj.addpoint(tt, **names) # interpolate, unless requested; # saves a few manual calls if interp: traj.interpolate() if lin: print("linearizing...") self.lintraj = traj self.regulator = LQR(self.tlims, traj.A, traj.B) self.regulator.solve() traj.feasible = True traj.tlims = self.tlims traj.jumps = jumps return traj @timeout(30000) def project(self, traj, tlims=None, lin=False): if traj.feasible: return traj if tlims is None: tlims = self.tlims self.xinit = traj.x(tlims[0]) if 'regulator' in self.__dict__: # print("regular projection") ltj = self.lintraj reg = self.regulator control = Controller(reference=traj, K=reg.K) self.set_u(control) # print(lin) return self.integrate(linearize=lin) else: print("integrating and linearizing for the first time") control = Controller(reference=traj) self.set_u(control) nutraj = self.integrate(linearize=True) return self.project(nutraj, tlims=tlims, lin=lin)
#Q = make_symmetric_random_psd(state_dim) #R = make_symmetric_random_psd(control_dim) Q = np.identity(state_dim) R = np.identity(control_dim) dyn_func = linear_dynamics(A, B); cost_func = quadtratic_cost(Q, R); T = 4 x0 = np.asarray((3, 2, 1)) #x0 = np.random.random(state_dim) lqr = LQR(A, B, Q, R, T) lqr.solve() lqr_states, lqr_controls, lqr_costs = lqr.forward_pass(x0) #Us2 = np.random.random((T, control_dim)) Us2 = np.ones((T, control_dim)) #Us2 = np.asarray(lqr_controls) xt = x0.copy(); Xs2 = xt; for t in range(0, T-1): xt1 = dyn_func(xt, Us2[t]) Xs2 = np.vstack((Xs2, xt1)) xt = xt1 #Xs = lqr_states; Us = lqr_controls; Xs = Xs2; Us = Us2; original_ilqr_cost = np.sum([cost_func(x, u) for (x,u) in zip(Xs, Us)]) ilqr = iLQR(dyn_func, cost_func, Xs, Us);
class System(object): """ a collection of callables, mostly """ def __init__(self, func, tlims=(0, 10), **kwargs): # TODO add an option to pass a symSystem directly # skipping a bunch of middleman bullshit that might # otherwise have to happen self.tlims = tlims keys = kwargs.keys() if 'xinit' in keys: self.xinit = kwargs['xinit'] self.dim = len(self.xinit) elif 'dim' in keys: self.dim = kwargs['dim'] else: self.dim = 1 self.f = func self.ufun = kwargs['controller'] if 'controller' in keys else None self.dfdx = kwargs['dfdx'] if 'dfdx' in keys else None self.dfdu = kwargs['dfdu'] if 'dfdu' in keys else None self.phi = kwargs['phi'] if 'phi' in keys else None self.delf = kwargs['delf'] if 'delf' in kwargs else None if self.ufun is None: self.dimu = 0 else: self.dimu = len(self.dimu(tlims[0])) # to be called after a reference has been set def build_cost(self, **kwargs): self.cost = CostFunction(self.dim, self.dimu, self.ref, projector=self.project, **kwargs) return self.cost def set_u(self, controller): if 'ufun' in self.__dict__.keys(): self._uold = self.ufun self.ufun = controller def reset_u(self): if '_uold' in self.__dict__.keys(): self.u = self._uold else: print("Nothing to reset to. Not doing anything.") # TODO make sure this works in all combinations of linearization # or not, and controlled or not; # Major cleanup needed. def integrate(self, use_jac=False, linearize=True, interpolate=True, **kwargs): keys = kwargs.keys() xinit = kwargs['xinit'] if 'xinit' in keys else self.xinit lin = linearize interp = interpolate if self.ufun is not None: func = lambda t, x: self.f(t, x, self.ufun(t, x)) dfdx = lambda t, x: self.dfdx(t, x, self.ufun(t, x)) dfdu = lambda t, x: self.dfdu(t, x, self.ufun(t, x)) else: func = self.f dfdx = self.dfdx dfdu = self.dfdu jac = dfdx if use_jac else None #Tracer()() if self.delf is not None: delfunc = lambda t, x: self.delf(t, x, self.ufun(t, x)) (t, x, jumps) = sysIntegrate(func, self.xinit, tlims=self.tlims, phi=self.phi, jac=jac, delfunc=delfunc) else: (t, x, jumps) = sysIntegrate(func, self.xinit, tlims=self.tlims, phi=self.phi, jac=jac) #Tracer()() components = ['x'] if 'ufun' in self.__dict__.keys(): components.append('u') if lin: components.append('A') components.append('B') traj = Trajectory(*components) for (tt, xx) in zip(t, x): names = {'x': xx} if 'u' in components: names['u'] = self.ufun(tt, xx) if 'A' in components: names['A'] = dfdx(tt, xx) if 'B' in components: names['B'] = dfdu(tt, xx) traj.addpoint(tt, **names) # interpolate, unless requested; # saves a few manual calls if interp: traj.interpolate() if lin: print("linearizing...") self.lintraj = traj self.regulator = LQR(self.tlims, traj.A, traj.B) #, jumps=jumps) self.regulator.solve() traj.feasible = True traj.tlims = self.tlims traj.jumps = jumps return traj @timeout(100) def project(self, traj, tlims=None, lin=False): if traj.feasible: return traj if tlims is None: tlims = self.tlims self.xinit = traj.x(tlims[0]) if 'regulator' in self.__dict__: # print("regular projection") ltj = self.lintraj reg = self.regulator control = Controller(reference=traj, K=reg.K) self.set_u(control) # print(lin) return self.integrate(linearize=lin) else: print("integrating and linearizing for the first time") control = Controller(reference=traj) self.set_u(control) nutraj = self.integrate(linearize=True) return self.project(nutraj, tlims=tlims, lin=lin)