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)
