def __init__(self, dynamics, cost, us0, x0, dt, max_step,
integrator=None, Ks=None):
self.dynamics = dynamics
if integrator is None:
self.integrator = EulerIntegrator(self.dynamics)
else:
self.integrator = integrator
self.cost = cost
self.us = us0 # N x m
self.min_hessian_value = 1.0 / max_step
N = self.cost.N
n = self.integrator.dynamics.n
m = self.integrator.dynamics.m
self.w = np.zeros(n) # Mean noise
if Ks is not None:
self.Ks = Ks
else:
self.Ks = np.zeros((N, m, n+1))
self.xs = np.empty((N + 1, n))
self.xs[0] = x0
self.dt = dt
self.us_up = np.empty_like(us0)
self.xs_up = np.empty_like(self.xs)
self.xs_up[0] = x0
self.V = np.Inf # Total value function
self.N = N
self.status = True
self.step_search = Armiho()
# Initialization
self.update_dynamics(self.us, self.xs)
def testResidualNoNoise(self):
dynamics = LinearDynamics(
(np.array([[1, 0], [0, 1]]), np.array([[1, 0], [0, 1]])))
integrator = EulerIntegrator(dynamics)
Sigma0 = Ellipsoid(np.eye(2), np.array([0, 0]))
Sigmaw = Ellipsoid(np.eye(2), np.array([0, 0]))
N = 10
h = 0.1
x0 = np.array([1, 1])
# Q, R, Qf
cost_gains = [np.diag([1, 1]), np.diag([1, 1]), np.diag([2, 2])]
cost_sqrt_gains = [np.sqrt(gain) for gain in cost_gains]
# Obstacles
obstacles = [
Obstacle(np.array([2, 2]), 1.2),
Obstacle(np.array([3, 3]), 0.2)
]
# Desired States, Controls:
xds = None
uds = np.zeros((N, 2))
# Controls:
us = np.zeros((N, 2))
us[:, 0] = 1.0
projection_matrix = np.eye(2)
feedback_gains = np.zeros((N, 2, 2))
residual_vector = residual(us.ravel(), h, N, x0, Sigma0, Sigmaw, xds,
uds, integrator, cost_sqrt_gains,
feedback_gains, obstacles,
projection_matrix)
Rsqrt = cost_sqrt_gains[1]
# Nominal dynamics
xs, _ = getNominalDynamics(us.ravel(), h, N, x0, integrator)
index = 0
for i in range(N):
np_testing.assert_almost_equal(residual_vector[index:(index + 2)],
np.dot(Rsqrt, us[i]))
index = index + 2
for obstacle in obstacles:
obs_distance = np.linalg.norm(xs[i] - obstacle.center)
if obs_distance > obstacle.radius:
np_testing.assert_equal(residual_vector[index:(index + 2)],
np.zeros(2))
else:
self.assertGreater(
np.linalg.norm(residual_vector[index:(index + 2)]), 0)
index = index + 2
# Terminal
for obstacle in obstacles:
obs_distance = np.linalg.norm(xs[-1] - obstacle.center)
if obs_distance > obstacle.radius:
np_testing.assert_equal(residual_vector[index:(index + 2)],
np.zeros(2))
else:
self.assertGreater(
np.linalg.norm(residual_vector[index:(index + 2)]), 0)
index = index + 2
self.assertEqual(index, residual_vector.size)
def test_lqr_linear_system(self):
dynamics = LinearDynamics((np.array([[0,1],[0,0]]), np.array([[0],[1]])))
integrator = EulerIntegrator(dynamics)
N = 10
h = 0.1
x0 = np.array([1,1])
# Q, R, Qf
cost_gains = [np.diag([1,1]), np.diag([1]), np.diag([2,2])]
feedback_gains = np.empty((N, 1, 2))
lqrUpdateGains(np.random.sample(N), h, N, x0, cost_gains, integrator,feedback_gains)
np_testing.assert_array_less(feedback_gains[:-1, :, :], np.zeros_like(feedback_gains)[:-1, :, :])
def test_lqr_unicycle(self):
dynamics = UnicycleDynamics()
integrator = EulerIntegrator(dynamics)
N = 10
h = 0.1
x0 = np.array([1,1,0])
# Q, R, Qf
cost_gains = [np.diag([1,1,1]), np.diag([0.5, 1.0]), np.diag([2,2,2])]
feedback_gains = np.empty((N, 2, 3))
u = np.ravel(np.vstack((np.ones(N), np.hstack((np.ones(int(0.5*N)), -np.ones(int(N-0.5*N)))))).T)
lqrUpdateGains(u, h, N, x0, cost_gains, integrator,feedback_gains)
from robust_gnn.gn_lqr_algo import gn_lqr_algo
from robust_gnn.obstacle_residual import Obstacle
from robust_gnn.ellipsoid import Ellipsoid
from optimal_control_framework.discrete_integrators import EulerIntegrator
from optimal_control_framework.dynamics import LinearDynamics
if __name__ == '__main__':
dynamics = LinearDynamics((np.array([[0, 1], [0, 0]]), np.array([[0],
[1]])))
N = 10
h = 0.1
u0 = np.zeros(N)
x0 = np.array([0, 0])
Sigma0 = Ellipsoid(np.eye(2), np.array([0.1, 0.1]))
Sigmaw = Ellipsoid(np.eye(2), np.array([0, 0]))
integrator = EulerIntegrator(dynamics)
# Q, R, Qf
cost_gains = [
np.diag([1, 1]),
np.diag([0.1]), 100 * np.array([[2, 0.2], [0.2, 2]])
]
# Obstacles
obstacles = [Obstacle(np.array([1]), 0.5)]
projection_matrix = np.array([[0, 1]])
ko_sqrt = 2
# Desired
xds = None
uds = np.zeros((N, 1))
xd_N = np.array([0.5, 0])
# Run algo and get outputs
out = gn_lqr_algo(N, h, x0, u0, Sigma0, Sigmaw, integrator, cost_gains,
class Ddp(object):
def __init__(self, dynamics, cost, us0, x0, dt, max_step,
integrator=None, Ks=None):
self.dynamics = dynamics
if integrator is None:
self.integrator = EulerIntegrator(self.dynamics)
else:
self.integrator = integrator
self.cost = cost
self.us = us0 # N x m
self.min_hessian_value = 1.0 / max_step
N = self.cost.N
n = self.integrator.dynamics.n
m = self.integrator.dynamics.m
self.w = np.zeros(n) # Mean noise
if Ks is not None:
self.Ks = Ks
else:
self.Ks = np.zeros((N, m, n+1))
self.xs = np.empty((N + 1, n))
self.xs[0] = x0
self.dt = dt
self.us_up = np.empty_like(us0)
self.xs_up = np.empty_like(self.xs)
self.xs_up[0] = x0
self.V = np.Inf # Total value function
self.N = N
self.status = True
self.step_search = Armiho()
# Initialization
self.update_dynamics(self.us, self.xs)
def regularize(self, Q, min_hessian_value):
w, v = np.linalg.eigh(Q)
min_w = np.min(w)
if min_w < min_hessian_value:
delta_reg = min_hessian_value - min_w
return Q + delta_reg * np.eye(Q.shape[0])
else:
return Q
def control(self, k, x):
delta_x = x - self.xs[k]
K_k = self.Ks[k]
u = self.us[k] + np.dot(K_k[:, :-1], delta_x)
return u
def update_dynamics(self, us, xs):
self.V = 0
for i, u in enumerate(us):
x = xs[i]
self.V = self.V + self.cost.stagewise_cost(i, x, u)
xs[i + 1] = self.integrator.step(i, self.dt, x, u, self.w)
# TODO Change instead of dynamics take in an integrator that
# integrates continuous dynamics using a fancy integrator maybe
self.V = self.V + self.cost.terminal_cost(xs[-1])
def backward_pass(self, xs, us, Ks, min_hessian_value):
V, Vx, Vxx = self.cost.terminal_cost(xs[-1], True)
dV = [0, 0]
for k in range(self.N - 1, -1, -1):
L, jac, hess = self.cost.stagewise_cost(k, xs[k], us[k], True)
Lx, Lu = jac
Lxx, Luu, Lxu = hess
fx, fu, _ = self.integrator.jacobian(k, self.dt, xs[k], us[k],
self.w)
Qx = Lx + np.dot(fx.T, Vx)
Qu = Lu + np.dot(fu.T, Vx)
Qxx = Lxx + np.dot(fx.T, np.dot(Vxx, fx))
Quu = Luu + np.dot(fu.T, np.dot(Vxx, fu))
Qux = Lxu + np.dot(fu.T, np.dot(Vxx, fx))
Quu_reg = self.regularize(Quu, min_hessian_value)
Qux_u = np.hstack((Qux, Qu[:, np.newaxis]))
K = -scipy_linalg.solve(Quu_reg, Qux_u, sym_pos=True)
Vx = Qx + np.dot(K[:, :-1].T, Qu)
Vxx = Qxx + np.dot(K[:, :-1].T, Qux)
Ks[k] = K
dV[0] = dV[0] + np.dot(K[:, -1], Qu)
dV[1] = dV[1] + 0.5*np.dot(K[:, -1], np.dot(Quu_reg, K[:, -1]))
self.step_search.dV = dV
def forward_pass_step(self, Ks, xs, us, alpha):
Vnew = 0
for k in range(self.N):
x = self.xs_up[k]
delta_x = x - self.xs[k]
K_k = Ks[k]
u = self.us[k] + alpha * K_k[:, -1] + np.dot(K_k[:, :-1], delta_x)
Vnew = Vnew + self.cost.stagewise_cost(k, x, u)
self.xs_up[k+1] = self.integrator.step(k, self.dt, x, u, self.w)
self.us_up[k] = u
Vnew = Vnew + self.cost.terminal_cost(self.xs_up[-1])
return Vnew
def forward_pass(self, Ks):
Vnew = np.Inf
alpha = self.step_search.alpha_max
self.step_search.step_converged = False
while not self.step_search.termination(self.V, Vnew, alpha):
Vnew = self.forward_pass_step(Ks, self.xs, self.us, alpha)
alpha = self.step_search.updateStep(self.V, Vnew, alpha)
if alpha < self.step_search.alpha_min:
print("Failed to find a reasonable step size! Probably converged!")
Vnew = self.V
self.status = False
else:
self.xs, self.xs_up = self.xs_up, self.xs # Swap xs
self.us, self.us_up = self.us_up, self.us # Swap us
self.status = True
return Vnew
def iterate(self):
self.backward_pass(self.xs, self.us, self.Ks,
self.min_hessian_value)
self.V = self.forward_pass(self.Ks)