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)
def setUp(self):
self.dynamics = UnicycleDynamics()
# Trajectory info
self.dt = 0.05
self.N = 20
Q = self.dt*np.zeros(self.dynamics.n)
R = 0.01*self.dt*np.eye(self.dynamics.m)
Qf = 100*np.eye(self.dynamics.n)
self.xd = np.array([1, 0.5, np.pi/2])
self.cost = LQRCost(self.N, Q, R, Qf, self.xd)
self.max_step = 0.5 # Allowed step for control
class TestUnicycleController(unittest.TestCase):
def setUp(self):
self.unicycle_dynamics = UnicycleDynamics()
self.n = self.unicycle_dynamics.n
self.m = self.unicycle_dynamics.m
self.controller = UnicycleController(self.unicycle_dynamics)
self.plot = True
def testControl(self):
#x0 = -1*np.ones(self.n)
x0 = 2 * np.ones(self.n)
x0[2] = 1
max_iters = 1000
iters = 0
converged = False
x = x0
dt = 0.1
t = 0
xs = []
ts = []
while iters < max_iters:
iters = iters + 1
u = self.controller.control(t, x)
xdot = self.unicycle_dynamics.xdot(t, x, u, np.zeros(self.n))
x = x + xdot * dt
t = t + dt
xs.append(x)
ts.append(t)
error = np.linalg.norm(x[:2])
if error < 1e-2:
converged = True
break
xs = np.vstack(xs)
ts = np.array(ts)
if self.plot:
plt.figure(1)
plt.subplot(2, 1, 1)
plt.plot(xs[:, 0], xs[:, 1], 'r')
plt.plot(0, 0, 'b*')
plt.subplot(2, 1, 2)
theta = np.remainder(xs[:, 2], 2 * np.pi)
inds_theta_greater = theta > np.pi
inds_theta_smaller = theta < -np.pi
theta[inds_theta_greater] = theta[inds_theta_greater] - 2 * np.pi
theta[inds_theta_smaller] = theta[inds_theta_smaller] + 2 * np.pi
plt.plot(ts, theta, 'r')
theta_d = np.arctan2(-xs[:, 1], -xs[:, 0])
plt.plot(ts, theta_d, 'b')
plt.show(block=True)
self.assertTrue(converged)
def setUp(self):
self.unicycle_dynamics = UnicycleDynamics()
self.n = self.unicycle_dynamics.n
self.m = self.unicycle_dynamics.m
self.controller = UnicycleController(self.unicycle_dynamics)
self.plot = True
#!/usr/bin/env python
import numpy as np
import matplotlib.pyplot as plt
from robust_gnn.plot_ellipse import plotEllipseArray, plotObstacleArray
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 UnicycleDynamics
if __name__ == "__main__":
dynamics = UnicycleDynamics()
N = 10
h = 0.1
n = 3
m = 2
x0 = np.array([0, 0, 0])
us0 = np.zeros((N, 2))
Sigma0 = Ellipsoid(np.eye(n), np.array([0.1, 0.1, 0.01]))
Sigmaw = Ellipsoid(np.eye(n), np.array([0, 0, 0]))
integrator = EulerIntegrator(dynamics)
# Q, R, Qf
cost_gains = [
np.diag([0, 0, 0]),
np.diag([2, 0.05]), 200 * np.diag([2, 2, 1])
]
# Obstacles
obstacles = [Obstacle(np.array([0.5, 0.1]), 0.1)]
projection_matrix = np.array([[1, 0, 0], [0, 1, 0]])
ko_sqrt = 20
# Desired