def backward_pass(self):
lc = LinearControl(self.nb_xdim, self.nb_udim, self.nb_steps)
xvalue = QuadraticStateValue(self.nb_xdim, self.nb_steps + 1)
xvalue.V[..., -1] = self.cost.Cxx[..., -1]
xvalue.v[..., -1] = self.cost.cx[..., -1]
for t in range(self.nb_steps - 2, -1, -1):
_Qxx = self.cost.Cxx[..., t] + self.dyn.A[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.A[..., t]
_Quu = self.cost.Cuu[..., t] + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.B[..., t]
_Qux = self.cost.Cxu[..., t].T + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.A[..., t]
_qx = self.cost.cx[..., t] + self.dyn.A[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.c[..., t] +\
self.dyn.A[..., t].T @ xvalue.v[..., t + 1]
_qu = self.cost.cu[..., t] + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.c[..., t] +\
self.dyn.B[..., t].T @ xvalue.v[..., t + 1]
_Quu_inv = np.linalg.inv(_Quu)
lc.K[...,t] = - _Quu_inv @ _Qux
lc.kff[..., t] = - _Quu_inv @ _qu
xvalue.V[..., t] = _Qxx + _Qux.T * lc.K[..., t]
xvalue.v[..., t] = _qx + lc.kff[..., t].T @ _Qux
return lc, xvalue
def __init__(self, env, nb_steps,
activation=range(-1, 0)):
self.env = env
# expose necessary functions
self.env_dyn = self.env.unwrapped.dynamics
self.env_cost = self.env.unwrapped.cost
self.env_init = self.env.unwrapped.init
self.ulim = self.env.action_space.high
self.nb_xdim = self.env.observation_space.shape[0]
self.nb_udim = self.env.action_space.shape[0]
self.nb_steps = nb_steps
# reference trajectory
self.xref = np.zeros((self.nb_xdim, self.nb_steps + 1))
self.xref[..., 0] = self.env_init()[0]
self.uref = np.zeros((self.nb_udim, self.nb_steps))
self.vfunc = QuadraticStateValue(self.nb_xdim, self.nb_steps + 1)
self.dyn = AnalyticalLinearDynamics(self.env_init, self.env_dyn, self.nb_xdim, self.nb_udim, self.nb_steps)
self.ctl = LinearControl(self.nb_xdim, self.nb_udim, self.nb_steps)
# activation of cost function
self.activation = np.zeros((self.nb_steps + 1,), dtype=np.int64)
self.activation[-1] = 1. # last step always in
self.activation[activation] = 1.
self.cost = AnalyticalQuadraticCost(self.env_cost, self.nb_xdim, self.nb_udim, self.nb_steps + 1)
def __init__(self, env, nb_steps, init_state):
self.env = env
# expose necessary functions
self.env_dyn = self.env.unwrapped.dynamics
self.env_cost = self.env.unwrapped.cost
self.env_init = init_state
self.ulim = self.env.action_space.high
self.dm_state = self.env.observation_space.shape[0]
self.dm_act = self.env.action_space.shape[0]
self.nb_steps = nb_steps
# reference trajectory
self.xref = np.zeros((self.dm_state, self.nb_steps + 1))
self.xref[..., 0] = self.env_init[0]
self.uref = np.zeros((self.dm_act, self.nb_steps))
self.vfunc = QuadraticStateValue(self.dm_state, self.nb_steps + 1)
self.dyn = AnalyticalLinearDynamics(self.env_dyn, self.dm_state,
self.dm_act, self.nb_steps)
self.ctl = LinearControl(self.dm_state, self.dm_act, self.nb_steps)
self.cost = AnalyticalQuadraticCost(self.env_cost, self.dm_state,
self.dm_act, self.nb_steps + 1)
def backward_pass(self):
lc = LinearControl(self.dm_state, self.dm_act, self.nb_steps)
xvalue = QuadraticStateValue(self.dm_state, self.nb_steps + 1)
xvalue.V[..., -1] = self.cost.Cxx[..., -1]
xvalue.v[..., -1] = self.cost.cx[..., -1]
for t in range(self.nb_steps - 1, -1, -1):
Qxx = self.cost.Cxx[..., t] + self.dyn.A[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.A[..., t]
Quu = self.cost.Cuu[..., t] + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.B[..., t]
Qux = self.cost.Cxu[..., t].T + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.A[..., t]
qx = self.cost.cx[..., t] + 2.0 * self.dyn.A[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.c[..., t] +\
self.dyn.A[..., t].T @ xvalue.v[..., t + 1]
qu = self.cost.cu[..., t] + 2.0 * self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.c[..., t] +\
self.dyn.B[..., t].T @ xvalue.v[..., t + 1]
Quu_inv = np.linalg.inv(Quu)
lc.K[..., t] = - Quu_inv @ Qux
lc.kff[..., t] = - 0.5 * Quu_inv @ qu
xvalue.V[..., t] = Qxx + Qux.T * lc.K[..., t]
xvalue.v[..., t] = qx + 2. * lc.kff[..., t].T @ Qux
return lc, xvalue
def __init__(self, env, nb_steps,
init_state, activation=None):
self.env = env
# expose necessary functions
self.env_dyn = self.env.unwrapped.dynamics
self.env_noise = self.env.unwrapped.sigma
self.env_cost = self.env.unwrapped.cost
self.env_init = init_state
self.ulim = self.env.action_space.high
self.dm_state = self.env.observation_space.shape[0]
self.dm_act = self.env.action_space.shape[0]
self.nb_steps = nb_steps
# reference trajectory
self.xref = np.zeros((self.dm_state, self.nb_steps + 1))
self.xref[..., 0] = self.env_init[0]
self.uref = np.zeros((self.dm_act, self.nb_steps))
self.vfunc = QuadraticStateValue(self.dm_state, self.nb_steps + 1)
self.dyn = AnalyticalLinearDynamics(self.env_dyn, self.dm_state, self.dm_act, self.nb_steps)
self.ctl = LinearControl(self.dm_state, self.dm_act, self.nb_steps)
# activation of cost function in shape of sigmoid
if activation is None:
self.weighting = np.ones((self.nb_steps + 1, ))
elif "mult" and "shift" in activation:
t = np.linspace(0, self.nb_steps, self.nb_steps + 1)
self.weighting = 1. / (1. + np.exp(- activation['mult'] * (t - activation['shift'])))
elif "discount" in activation:
self.weighting = np.ones((self.nb_steps + 1,))
gamma = activation["discount"] * np.ones((self.nb_steps, ))
self.weighting[1:] = np.cumprod(gamma)
else:
raise NotImplementedError
self.cost = AnalyticalQuadraticCost(self.env_cost, self.dm_state, self.dm_act, self.nb_steps + 1)
class Riccati:
def __init__(self, env, nb_steps,
init_state, activation=None):
self.env = env
# expose necessary functions
self.env_dyn = self.env.unwrapped.dynamics
self.env_noise = self.env.unwrapped.sigma
self.env_cost = self.env.unwrapped.cost
self.env_init = init_state
self.ulim = self.env.action_space.high
self.dm_state = self.env.observation_space.shape[0]
self.dm_act = self.env.action_space.shape[0]
self.nb_steps = nb_steps
# reference trajectory
self.xref = np.zeros((self.dm_state, self.nb_steps + 1))
self.xref[..., 0] = self.env_init[0]
self.uref = np.zeros((self.dm_act, self.nb_steps))
self.vfunc = QuadraticStateValue(self.dm_state, self.nb_steps + 1)
self.dyn = AnalyticalLinearDynamics(self.env_dyn, self.dm_state, self.dm_act, self.nb_steps)
self.ctl = LinearControl(self.dm_state, self.dm_act, self.nb_steps)
# activation of cost function in shape of sigmoid
if activation is None:
self.weighting = np.ones((self.nb_steps + 1, ))
elif "mult" and "shift" in activation:
t = np.linspace(0, self.nb_steps, self.nb_steps + 1)
self.weighting = 1. / (1. + np.exp(- activation['mult'] * (t - activation['shift'])))
elif "discount" in activation:
self.weighting = np.ones((self.nb_steps + 1,))
gamma = activation["discount"] * np.ones((self.nb_steps, ))
self.weighting[1:] = np.cumprod(gamma)
else:
raise NotImplementedError
self.cost = AnalyticalQuadraticCost(self.env_cost, self.dm_state, self.dm_act, self.nb_steps + 1)
def rollout(self, nb_episodes, env=None):
if env is None:
env = self.env
env_cost = self.env_cost
else:
env = env
env_cost = env.unwrapped.cost
data = {'x': np.zeros((self.dm_state, self.nb_steps, nb_episodes)),
'u': np.zeros((self.dm_act, self.nb_steps, nb_episodes)),
'xn': np.zeros((self.dm_state, self.nb_steps, nb_episodes)),
'c': np.zeros((self.nb_steps + 1, nb_episodes))}
for n in range(nb_episodes):
x = env.reset()
for t in range(self.nb_steps):
u = self.ctl.action(x, t)
data['u'][..., t, n] = u
# expose true reward function
c = env_cost(x, u, data['u'][..., t - 1, n], self.weighting[t])
data['c'][t] = c
data['x'][..., t, n] = x
x, _, _, _ = env.step(u)
data['xn'][..., t, n] = x
c = env_cost(x, np.zeros((self.dm_act, )), np.zeros((self.dm_act, )), self.weighting[-1])
data['c'][-1, n] = c
return data
def forward_pass(self, ctl):
state = np.zeros((self.dm_state, self.nb_steps + 1))
action = np.zeros((self.dm_act, self.nb_steps))
cost = np.zeros((self.nb_steps + 1, ))
state[..., 0] = self.env.reset()
for t in range(self.nb_steps):
action[..., t] = ctl.action(state[..., t], t)
cost[..., t] = self.env_cost(state[..., t], action[..., t], action[..., t - 1], self.weighting[t])
state[..., t + 1], _, _, _ = self.env.step(action[..., t])
cost[..., -1] = self.env_cost(state[..., -1], np.zeros((self.dm_act, )),
np.zeros((self.dm_act, )), self.weighting[-1])
return state, action, cost
def backward_pass(self):
lc = LinearControl(self.dm_state, self.dm_act, self.nb_steps)
xvalue = QuadraticStateValue(self.dm_state, self.nb_steps + 1)
xvalue.V[..., -1] = self.cost.Cxx[..., -1]
xvalue.v[..., -1] = self.cost.cx[..., -1]
for t in range(self.nb_steps - 1, -1, -1):
Qxx = self.cost.Cxx[..., t] + self.dyn.A[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.A[..., t]
Quu = self.cost.Cuu[..., t] + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.B[..., t]
Qux = self.cost.Cxu[..., t].T + self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.A[..., t]
qx = self.cost.cx[..., t] + 2.0 * self.dyn.A[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.c[..., t] +\
self.dyn.A[..., t].T @ xvalue.v[..., t + 1]
qu = self.cost.cu[..., t] + 2.0 * self.dyn.B[..., t].T @ xvalue.V[..., t + 1] @ self.dyn.c[..., t] +\
self.dyn.B[..., t].T @ xvalue.v[..., t + 1]
Quu_inv = np.linalg.inv(Quu)
lc.K[..., t] = - Quu_inv @ Qux
lc.kff[..., t] = - 0.5 * Quu_inv @ qu
xvalue.V[..., t] = Qxx + Qux.T * lc.K[..., t]
xvalue.v[..., t] = qx + 2. * lc.kff[..., t].T @ Qux
return lc, xvalue
def plot(self, xref=None, uref=None):
xref = self.xref if xref is None else xref
uref = self.uref if uref is None else uref
import matplotlib.pyplot as plt
plt.figure()
t = np.linspace(0, self.nb_steps, self.nb_steps + 1)
for k in range(self.dm_state):
plt.subplot(self.dm_state + self.dm_act, 1, k + 1)
plt.plot(t, xref[k, :], '-b')
t = np.linspace(0, self.nb_steps, self.nb_steps)
for k in range(self.dm_act):
plt.subplot(self.dm_state + self.dm_act, 1, self.dm_state + k + 1)
plt.plot(t, uref[k, :], '-g')
plt.show()
def run(self):
# get linear system dynamics around ref traj.
self.dyn.taylor_expansion(self.xref, self.uref)
# get quadratic cost around ref traj.
self.cost.taylor_expansion(self.xref, self.uref, self.weighting)
# backward pass to get ctrl.
self.ctl, self.vfunc = self.backward_pass()
# forward pass to get cost and traj.
self.xref, self.uref, _cost = self.forward_pass(self.ctl)
return np.sum(_cost)