def test_state_hessian(self):
"""
"""
g_xs = np.ones((1, 3)) * 0.5
xs = np.ones((1, 3))
cost_hess =\
TwoWheeledConfigModule.hessian_cost_fn_with_state(xs, g_xs)
# numeric grad
eps = 1e-4
expected_hess = np.zeros((1, 3, 3))
for i in range(3):
tmp_x = xs.copy()
tmp_x[0, i] = xs[0, i] + eps
forward = \
TwoWheeledConfigModule.gradient_cost_fn_with_state(
tmp_x, g_xs, terminal=False)
tmp_x = xs.copy()
tmp_x[0, i] = xs[0, i] - eps
backward = \
TwoWheeledConfigModule.gradient_cost_fn_with_state(
tmp_x, g_xs, terminal=False)
expected_hess[0, :, i] = (forward - backward) / (2. * eps)
assert cost_hess == pytest.approx(expected_hess)
def test_calc_costs(self):
# make config
config = TwoWheeledConfigModule()
# set
pred_len = 5
state_size = 3
input_size = 2
pop_size = 2
pred_xs = np.ones((pop_size, pred_len, state_size))
g_xs = np.ones((pop_size, pred_len, state_size)) * 0.5
input_samples = np.ones((pop_size, pred_len, input_size)) * 0.5
costs = config.input_cost_fn(input_samples)
expected_costs = np.ones((pop_size, pred_len, input_size))*0.5
assert costs == pytest.approx(expected_costs**2 * np.diag(config.R))
costs = config.state_cost_fn(pred_xs, g_xs)
expected_costs = np.ones((pop_size, pred_len, state_size))*0.5
assert costs == pytest.approx(expected_costs**2 * np.diag(config.Q))
costs = config.terminal_state_cost_fn(pred_xs[:, -1, :],\
g_xs[:, -1, :])
expected_costs = np.ones((pop_size, state_size))*0.5
assert costs == pytest.approx(expected_costs**2 * np.diag(config.Sf))
def test_input_gradient(self):
"""
"""
us = np.ones((1, 2))
cost_grad =\
TwoWheeledConfigModule.gradient_cost_fn_with_input(None, us)
# numeric grad
eps = 1e-4
expected_grad = np.zeros((1, 2))
for i in range(2):
tmp_u = us.copy()
tmp_u[0, i] = us[0, i] + eps
forward = \
TwoWheeledConfigModule.input_cost_fn(tmp_u)
forward = np.sum(forward)
tmp_u = us.copy()
tmp_u[0, i] = us[0, i] - eps
backward = \
TwoWheeledConfigModule.input_cost_fn(tmp_u)
backward = np.sum(backward)
expected_grad[0, i] = (forward - backward) / (2. * eps)
assert cost_grad == pytest.approx(expected_grad)
def test_state_gradient(self):
"""
"""
xs = np.ones((1, 3))
g_xs = np.ones((1, 3)) * 0.5
cost_grad =\
TwoWheeledConfigModule.gradient_cost_fn_with_state(xs, g_xs)
# numeric grad
eps = 1e-4
expected_grad = np.zeros((1, 3))
for i in range(3):
tmp_x = xs.copy()
tmp_x[0, i] = xs[0, i] + eps
forward = \
TwoWheeledConfigModule.state_cost_fn(tmp_x, g_xs)
forward = np.sum(forward)
tmp_x = xs.copy()
tmp_x[0, i] = xs[0, i] - eps
backward = \
TwoWheeledConfigModule.state_cost_fn(tmp_x, g_xs)
backward = np.sum(backward)
expected_grad[0, i] = (forward - backward) / (2. * eps)
assert cost_grad == pytest.approx(expected_grad)
def test_input_hessian(self):
"""
"""
us = np.ones((1, 2))
xs = np.ones((1, 3))
cost_hess = TwoWheeledConfigModule.hessian_cost_fn_with_input(us, xs)
# numeric grad
eps = 1e-4
expected_hess = np.zeros((1, 2, 2))
for i in range(2):
tmp_u = us.copy()
tmp_u[0, i] = us[0, i] + eps
forward = \
TwoWheeledConfigModule.gradient_cost_fn_with_input(
xs, tmp_u)
tmp_u = us.copy()
tmp_u[0, i] = us[0, i] - eps
backward = \
TwoWheeledConfigModule.gradient_cost_fn_with_input(
xs, tmp_u)
expected_hess[0, :, i] = (forward - backward) / (2. * eps)
assert cost_hess == pytest.approx(expected_hess)
def test_gradient_input(self):
config = TwoWheeledConfigModule()
two_wheeled_model = TwoWheeledModel(config)
xs = np.ones((1, config.STATE_SIZE))
xs[0, -1] = np.pi / 6.
us = np.ones((1, config.INPUT_SIZE))
grad = two_wheeled_model.calc_f_u(xs, us, config.DT)
# expected cost
expected_grad = np.zeros((1, config.STATE_SIZE, config.INPUT_SIZE))
eps = 1e-4
for i in range(config.INPUT_SIZE):
tmp_u = us.copy()
tmp_u[0, i] = us[0, i] + eps
forward = \
two_wheeled_model.predict_next_state(xs[0], tmp_u[0])
tmp_u = us.copy()
tmp_u[0, i] = us[0, i] - eps
backward = \
two_wheeled_model.predict_next_state(xs[0], tmp_u[0])
expected_grad[0, :, i] = (forward - backward) / (2. * eps)
assert grad == pytest.approx(expected_grad)
def test_predict_traj(self):
config = TwoWheeledConfigModule()
two_wheeled_model = TwoWheeledModel(config)
curr_x = np.ones(config.STATE_SIZE)
curr_x[-1] = np.pi / 6.
u = np.ones((1, config.INPUT_SIZE))
pred_xs = two_wheeled_model.predict_traj(curr_x, u)
u = np.tile(u, (1, 1, 1))
pred_xs_alltogether = two_wheeled_model.predict_traj(curr_x, u)[0]
assert pred_xs_alltogether == pytest.approx(pred_xs)
def test_step(self):
config = TwoWheeledConfigModule()
two_wheeled_model = TwoWheeledModel(config)
curr_x = np.ones(config.STATE_SIZE)
curr_x[-1] = np.pi / 6.
u = np.ones((1, config.INPUT_SIZE))
next_x = two_wheeled_model.predict_traj(curr_x, u)
pos_x = np.cos(curr_x[-1]) * u[0, 0] * config.DT + curr_x[0]
pos_y = np.sin(curr_x[-1]) * u[0, 0] * config.DT + curr_x[1]
expected = np.array([[1., 1., np.pi / 6.],
[pos_x, pos_y, curr_x[-1] + u[0, 1] * config.DT]])
assert next_x == pytest.approx(expected)
