class TestLinearDynamics(unittest.TestCase):
def setUp(self):
self.n = 5
self.m = 2
A = np.ones([self.n, self.n])
B = np.ones([self.n, self.m])
self.dynamics = LinearDynamics([A, B])
def testProperties(self):
self.assertEqual(self.dynamics.n, self.n)
self.assertEqual(self.dynamics.m, self.m)
def testXdot(self):
x = np.ones(self.n)
u = np.ones(self.m)
w = np.zeros(self.n)
xdot = self.dynamics.xdot(1.0, x, u, w)
# print xdot
self.assertEqual(len(xdot), self.n)
np_testing.assert_allclose(xdot, 7.0 * np.ones(self.n), atol=1e-8)
def testJacobian(self):
x = np.ones(self.n)
u = np.ones(self.m)
w = np.zeros(self.n)
jac = self.dynamics.jacobian(1.0, x, u, w)
self.assertEqual(jac[0].shape, (self.n, self.n))
self.assertEqual(jac[1].shape, (self.n, self.m))
self.assertEqual(jac[2].shape, (self.n, self.n))
def testSampling():
n = 2
m = 1
M = 10
tf = 1
dt = 0.1
ts = np.arange(0, tf + dt, dt)
N = len(ts) - 1
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
Q = np.ones(n)
Qf = 2 * np.ones(n)
R = np.ones(m)
us = np.zeros([N, m])
dynamics = LinearDynamics([A, B])
controller = FeedforwardController(us)
cost = LQRCost(N, Q, R, Qf)
# No noise
ws_sampling_fun = lambda *args: np.zeros(n)
x0_sampling_fun = lambda *args: np.zeros(n)
integrator = EulerIntegrator(dynamics)
# Create sampler
sample_trajectories = SampleTrajectories(dynamics, integrator, cost,
ws_sampling_fun, x0_sampling_fun)
# Sample M trajectories
xss, uss, Jss = sample_trajectories.sample(M, ts, controller)
assert (xss.shape == (M, N + 1, n))
assert (uss.shape == (M, N, m))
assert (Jss.shape == (M, N))
np_testing.assert_allclose(xss[0], xss[-1], atol=1e-8)
np_testing.assert_allclose(Jss[0], Jss[-1], atol=1e-8)
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 setUp(self):
self.n = 5
self.m = 2
N = 10
A = np.ones([self.n, self.n])
B = np.ones([self.n, self.m])
self.dynamics = LinearDynamics([A, B])
Ks = np.ones([N, self.n, self.m])
ks = np.ones([N, self.m])
Sigmas = 1e-3 * np.ones([N, self.m])
self.controller = LinearController(self.dynamics, Ks, ks, Sigmas)
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 setUp(self):
self.n = 2
self.m = 1
A = np.zeros([self.n, self.n])
A[0, 1] = 1
B = np.zeros([self.n, self.m])
B[1, 0] = 1
self.dynamics = LinearDynamics([A,B])
# Trajectory info
self.dt = 0.1
self.N = 10
Q = self.dt*np.zeros(self.n)
R = 0.01*self.dt*np.eye(self.m)
Qf = 100*np.eye(self.n)
self.xd = np.array([1, 0])
self.cost = LQRCost(self.N, Q, R, Qf, self.xd)
self.max_step = 0.5 # Allowed step for control
def testLinearization(): n = 5 m = 2 A = np.ones([n, n]) B = np.ones([n, m]) x0 = np.array([0,0,0,0,1]) ts = np.arange(0, 1.1, 0.1) N = len(ts) - 1 us = np.zeros([N, m]) dynamics = LinearDynamics([A,B]) controller = FeedforwardController(us) linearize_dynamics = LinearizeDynamics(dynamics) wbar_sample_fun = lambda *args : np.zeros(n) As, Bs, Cs = linearize_dynamics.linearize(ts, controller, x0, wbar_sample_fun) np_testing.assert_allclose(As[0], np.eye(n) + 0.1*A, atol=1e-8) np_testing.assert_allclose(Bs[0], 0.1*B, atol=1e-8) np_testing.assert_allclose(Cs[0], 0.1*np.eye(n), atol=1e-8)
def testIntegration(): n = 2 m = 1 tf = 1 dt = 0.1 ts = np.arange(0, tf+dt, dt) N = len(ts)-1 A = np.array([[0, 1],[0, 0]]) B = np.array([[0], [1]]) x0 = np.array([0,1]) us = np.zeros([N, m]) dynamics = LinearDynamics([A,B]) controller = FeedforwardController(us) # No noise ws_sample_fun = lambda *args : np.zeros(n) integrator = EulerIntegrator(dynamics) xs, us_out = integrator.integrate(x0, ts, controller, ws_sample_fun) np_testing.assert_allclose(us_out, us,atol=1e-8) np_testing.assert_allclose(xs[-1], np.array([1, 1]), atol=1e-8)
#!/usr/bin/env python
import numpy as np
import matplotlib.pyplot as plt
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))
def setUp(self):
self.n = 5
self.m = 2
A = np.ones([self.n, self.n])
B = np.ones([self.n, self.m])
self.dynamics = LinearDynamics([A, B])
from optimal_control_framework.dynamics import LinearDynamics from robust_ddp.robust_ddp import RobustDdp from robust_ddp.robust_lqr_obstacle_cost import RobustLQRObstacleCost from ellipsoid_package.ellipsoid_projection import plotEllipse from ellipsoid_package.ellipsoid_helper import findEllipsoid import matplotlib.pyplot as plt import numpy as np np.set_printoptions(precision=3, suppress=True) n = 2 m = 1 A = np.zeros((2, 2)) A[0, 1] = 1 B = np.array([[0], [1]]) dynamics = LinearDynamics([A, B]) # Covariance Sigma0 = np.diag([0.2, 0.2]) Sigma_w = 0 * np.eye(n) # Trajectory info dt = 0.05 N = 40 #Q = np.zeros(dynamics.n) #Q[1] = 0*dt # For velocity along the trajectory R = 0.01 * dt * np.eye(dynamics.m) Qf = 50 * np.eye(dynamics.n) Q = Qf * dt * 0.1 ts = np.arange(N + 1) * dt xd = np.array([1, 0]) cost = RobustLQRObstacleCost(N, Q, R, Qf, xd, kSigma=0 * dt) max_step = 50.0 # Allowed step for control