def setUp(self):
self.N = 5 # Length of traj
self.n = 2 # lenght of state
self.m = 1 # Length of control
self.Q = np.ones(self.n)
self.Qf = 5 * np.ones(self.n)
self.R = 2 * np.ones(self.m)
self.cost = LQRCost(self.N, self.Q, self.R, self.Qf)
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 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 TestLQRCost(unittest.TestCase):
def setUp(self):
self.N = 5 # Length of traj
self.n = 2 # lenght of state
self.m = 1 # Length of control
self.Q = np.ones(self.n)
self.Qf = 5 * np.ones(self.n)
self.R = 2 * np.ones(self.m)
self.cost = LQRCost(self.N, self.Q, self.R, self.Qf)
def test_stagewise_cost(self):
self.assertEqual(
self.cost.stagewise_cost(0, np.ones(self.n), np.ones(self.m)), 4)
def test_terminal_cost(self):
self.assertEqual(self.cost.terminal_cost(np.ones(self.n)), 10.0)
def test_cumulative_cost(self):
Js = self.cost.cumulative_cost(np.ones([self.N + 1, self.n]),
np.ones([self.N, self.m]))
self.assertEqual(len(Js), self.N)
self.assertEqual(Js[-1], 14.0)
self.assertTrue(Js[0] >= Js[-1])
def test_cumulative_cost_different_terminal_state(self):
"""
Regression Test to ensure the cost is using the last state for terminal cost!
"""
xs = np.zeros([self.N + 1, self.n])
us = np.zeros([self.N, self.n])
# Set the terminal state to ones
xs[self.N] = np.ones([self.n])
# Evaluate costs
Js = self.cost.cumulative_cost(xs, us)
# Last one is terminal cost
self.assertEqual(Js[-1], 10.0)
# Since other costs are 0
self.assertTrue(Js[0] == Js[-1])
class TestLQRCost(unittest.TestCase):
def setUp(self):
self.N = 5 # Length of traj
self.n = 2 # lenght of state
self.m = 1 # Length of control
self.Q = np.ones(self.n)
self.Qf = 5 * np.ones(self.n)
self.R = 2 * np.ones(self.m)
self.cost = LQRCost(self.N, self.Q, self.R, self.Qf)
def test_stagewise_cost(self):
self.assertEqual(self.cost.stagewise_cost(
0, np.ones(self.n), np.ones(self.m)), 2)
def test_terminal_cost(self):
self.assertEqual(self.cost.terminal_cost(np.ones(self.n)), 5.0)
def test_cumulative_cost(self):
Js = self.cost.cumulative_cost(
np.ones([self.N + 1, self.n]), np.ones([self.N, self.m]))
self.assertEqual(len(Js), self.N)
self.assertEqual(Js[-1], 7.0)
self.assertTrue(Js[0] >= Js[-1])
def test_cumulative_cost_different_terminal_state(self):
"""
Regression Test to ensure the cost is using the last state for terminal cost!
"""
xs = np.zeros([self.N + 1, self.n])
us = np.zeros([self.N, self.n])
# Set the terminal state to ones
xs[self.N] = np.ones([self.n])
# Evaluate costs
Js = self.cost.cumulative_cost(xs, us)
# Last one is terminal cost
self.assertEqual(Js[-1], 5.0)
# Since other costs are 0
self.assertTrue(Js[0] == Js[-1])
def test_stagewise_grads(self):
x = np.random.sample(self.n)
u = np.random.sample(self.m)
L, jac, hess = self.cost.stagewise_cost(0, x, u, True)
np_testing.assert_almost_equal(jac[0], self.Q * x)
np_testing.assert_almost_equal(jac[1], self.R * u)
np_testing.assert_almost_equal(np.diag(hess[0]), self.Q)
np_testing.assert_almost_equal(np.diag(hess[1]), self.R)
np_testing.assert_almost_equal(hess[2], 0)
def test_terminal_grads(self):
x = np.random.sample(self.n)
Lf, Qfx, Qfxx = self.cost.terminal_cost(x, True)
np_testing.assert_almost_equal(Qfx, self.Qf * x)
np_testing.assert_almost_equal(np.diag(Qfxx), self.Qf)
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
import numpy as np
np.set_printoptions(precision=3, suppress=True)
dynamics = SimpleCarDynamics()
# Trajectory info
dt = 0.05
N = 20
Q = dt * np.zeros(dynamics.n)
R = 0.01 * dt * np.eye(dynamics.m)
Qf = 100 * np.eye(dynamics.n)
Qf[-1, -1] = 1 # Don't care about end steering angle
ts = np.arange(N + 1) * dt
#xd = np.array([1, 0.5, np.pi/4, 0, 0])
xd = np.array([0.5, 0.5, 0, 0, 0])
#xd = np.array([1, 0, 0, 0, 0])
cost = LQRCost(N, Q, R, Qf, xd)
max_step = 10.0 # Allowed step for control
x0 = np.array([0, 0, 0, 0, 0])
us0 = np.zeros([N, dynamics.m])
ddp = Ddp(dynamics, cost, us0, x0, dt, max_step)
V = ddp.V
for i in range(50):
ddp.iterate()
V = ddp.V
print("V: ", V)
print("xn: ", ddp.xs[-1])
plt.figure(1)
plt.plot(ddp.xs[:, 0], ddp.xs[:, 1])
plt.plot(xd[0], xd[1], 'r*')
plt.xlabel('x (m)')