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 testEllipsoidPropagationStableDynamics(self):
Sigma_i = Ellipsoid(np.eye(2), np.array([1, 2]))
Sigma_w_i = Ellipsoid(np.eye(2), np.array([0.1, 0.01]))
# A, B, G
dynamics_params = [
np.array([[1, 0.1], [0, 0]]),
np.array([[0], [0.1]]),
np.eye(2)
]
feedback_gain = np.array([[-5, -5]])
for i in range(50):
Sigma_n = propagateEllipsoid(Sigma_i, Sigma_w_i, dynamics_params,
feedback_gain)
self.assertLess(np.linalg.norm(Sigma_n.S),
np.linalg.norm(Sigma_i.S))
Sigma_i = Sigma_n
def testRotatedEllipsoid(self):
obstacle = Obstacle(np.array([1, 1]), 2.0)
projection_matrix = np.array([[0, 1, 0], [0, 0, 1]])
R = tf.euler_matrix(np.pi / 4, 0, 0, 'rzyx')[:3, :3]
Sigma = Ellipsoid(R, np.array([0, 0.2, 0.2]))
# Finally z should be 0.2, y should be 0.2/sqrt(2)
constraint = obstacleConstraint(Sigma,
np.array([0, -1, -1]),
obstacle,
projection_matrix,
clip=False)
self.assertGreater(constraint, 0)
constraint = obstacleConstraint(Sigma,
np.array([0, 1, 3.2]),
obstacle,
projection_matrix,
clip=False)
self.assertEqual(constraint, 0)
constraint = obstacleConstraint(Sigma,
np.array([0, -1 - 0.2 / np.sqrt(2),
1]),
obstacle,
projection_matrix,
clip=False)
self.assertAlmostEqual(constraint, 0, 12)
def testUnRotatedProjection(self):
# R, S
ellipsoid = Ellipsoid(np.eye(3), np.array([4, 3, 2]))
T = np.array([[0, 1, 0], [0, 0, 1]])
projected_ellipsoid = projectEllipsoid(T, ellipsoid)
np_testing.assert_almost_equal(projected_ellipsoid.S, np.array([3, 2]))
np_testing.assert_almost_equal(projected_ellipsoid.R,
np.array([[1, 0], [0, 1]]))
def testRotatedProjection(self):
# R, S
ellipsoid = Ellipsoid(
tf.euler_matrix(np.pi / 3, np.pi / 4, np.pi / 6, 'rzyx')[:3, :3],
np.array([4, 5, 6]))
T = np.array([[0, 1, 0.5], [0.2, 0.3, 1]]) # Some linear projection
projected_ellipsoid = projectEllipsoid(T, ellipsoid)
N = 1000
samples = np.random.sample((3, N))
samples = samples / np.linalg.norm(samples, axis=0)
for sample in samples.T:
sample = np.dot(ellipsoid.R, ellipsoid.S * sample)
proj_sample = np.dot(T, sample)
# Verify constraint
rot_proj = np.dot(projected_ellipsoid.R.T, proj_sample)
rot_proj_scaled = rot_proj / projected_ellipsoid.S
self.assertLess(np.linalg.norm(rot_proj_scaled), 1.0)
def testUnRotatedEllipsoid(self):
obstacle = Obstacle(np.array([1, 1]), 2.0)
projection_matrix = np.array([[1, 0, 0], [0, 1, 0]])
Sigma = Ellipsoid(np.eye(3), np.array([0.2, 0.1, 0.6]))
# Inside
constraint = obstacleConstraint(Sigma,
np.zeros(3),
obstacle,
projection_matrix,
clip=False)
self.assertLess(constraint, 0)
# Passing through center
constraint = obstacleConstraint(Sigma,
np.ones(3),
obstacle,
projection_matrix,
clip=False)
self.assertLess(constraint, 0)
# Outside
constraint = obstacleConstraint(Sigma,
-1 * np.ones(3),
obstacle,
projection_matrix,
clip=False)
self.assertGreater(constraint, 0)
# Check residual with clipping
residual = obstacleResidual(Sigma,
-1 * np.ones(3),
obstacle,
projection_matrix,
clip=True)
np_testing.assert_equal(residual, np.zeros_like(residual))
# On boundary
constraint = obstacleConstraint(Sigma,
np.array([-1.2, 1, 0]),
obstacle,
projection_matrix,
clip=False)
self.assertEqual(constraint, 0)
# On boundary2
constraint = obstacleConstraint(Sigma,
np.array([1, 3.1, 0]),
obstacle,
projection_matrix,
clip=False)
self.assertEqual(constraint, 0)
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))
xd_N = np.array([0.5, 0])