def test_quadratize_non_quad_state(self, wayareas, cur_gaussians):
cur_states = np.zeros((4, 3))
for ii, gg in enumerate(cur_gaussians):
cur_states[:, ii] = gg.means[0, :]
for cov in wayareas.covariances:
cov[2, 2] = 100.
cov[3, 3] = 100.
wayareas.weights = [0.5, 0.5, 0.5, 0.5]
obj_num = 0
elqrGaussian = guide.ELQRGaussian(cur_gaussians=cur_gaussians,
wayareas=wayareas,
horizon_len=3)
exp_Q = np.array(
[[
6.08618287e-08, -7.62808745e-06, -1.31408886e-07,
-5.24016660e-10
],
[-7.62808745e-06, 9.56062598e-04, 1.64700683e-05, 6.56773710e-08],
[-1.31408886e-07, 1.64700683e-05, 2.83729487e-07, 1.13142255e-09],
[-5.24016660e-10, 6.56773710e-08, 1.13142255e-09,
4.51175171e-12]])
exp_q = np.array([[7.570267e-03], [1.793011e-05], [1.558761e-04],
[2.919833e-05]])
Q, q = elqrGaussian.quadratize_non_quad_state(all_states=cur_states,
obj_num=obj_num)
test.assert_allclose(Q, exp_Q, atol=3e-8)
test.assert_allclose(q, exp_q, atol=3e-8)
def test_iterate(self, cur_gaussians, wayareas, dyn_funcs, inv_dyn_funcs):
meas = GaussianMixture()
meas.means = [
np.array([[1], [2], [3], [4]]),
np.array([[4], [5], [6], [7]])
]
meas.covariances = [2 * np.eye(4), 2 * np.eye(4)]
meas.weights = [0.3, 0.3, 0.3]
dyn_lst = [dyn_funcs, dyn_funcs, dyn_funcs]
inv_dyn_lst = [inv_dyn_funcs, inv_dyn_funcs, inv_dyn_funcs]
n_inputs = [2, 2, 2]
for cov in wayareas.covariances:
cov[2, 2] = 100.
cov[3, 3] = 100.
wayareas.weights = [0.5, 0.5, 0.5, 0.5]
elqrGaussian = guide.ELQRGaussian(Q=np.eye(4),
R=np.eye(2),
cur_gaussians=cur_gaussians,
wayareas=wayareas,
horizon_len=3)
elqrGaussian.iterate(meas,
est_dyn_lst=dyn_lst,
est_inv_dyn_lst=inv_dyn_lst,
n_inputs_lst=n_inputs,
dt=1)
def test_constructor(self, dyn_funcs, inv_dyn_funcs, wayareas, Q):
Z_mat4 = np.zeros((4, 4))
Z_mat24 = np.zeros((2, 4))
Z_vec4 = np.zeros((4, 1))
Z_vec2 = np.zeros((2, 1))
gm1 = guide.GaussianObject()
gm1.dyn_functions = dyn_funcs
gm1.inv_dyn_functons = inv_dyn_funcs
gm1.means = np.vstack(
(Z_vec4.copy().T, Z_vec4.copy().T, Z_vec4.copy().T))
gm1.ctrl_inputs = np.vstack(
(Z_vec2.copy().T, Z_vec2.copy().T, Z_vec2.copy().T))
gm1.feedback_lst = [Z_mat24.copy(), Z_mat24.copy(), Z_mat24.copy()]
gm1.feedforward_lst = [Z_vec2.copy(), Z_vec2.copy(), Z_vec2.copy()]
gm1.cost_to_come_mat = [Z_mat4.copy(), Z_mat4.copy(), Z_mat4.copy()]
gm1.cost_to_come_vec = [Z_vec4.copy(), Z_vec4.copy(), Z_vec4.copy()]
gm1.cost_to_go_mat = gm1.cost_to_come_mat.copy()
gm1.cost_to_go_vec = gm1.cost_to_come_vec.copy()
gm1.covariance = Z_mat4.copy()
gm1.weight = 1
elqrGaussian = guide.ELQRGaussian(cur_gaussians=[gm1], horizon_len=3)
assert len(elqrGaussian.gaussians) == 1
assert len(elqrGaussian.targets.means) == 0
elqrGaussian = guide.ELQRGaussian(cur_gaussians=[gm1],
wayareas=wayareas,
Q=Q,
horizon_len=3)
assert len(elqrGaussian.gaussians) == 1
assert len(elqrGaussian.targets.means) == len(wayareas.means)
test.assert_allclose(elqrGaussian.state_penalty, Q)
elqrGaussian = guide.ELQRGaussian(horizon_len=3)
assert len(elqrGaussian.gaussians) == 0
assert elqrGaussian.state_penalty.size == 0
test.assert_approx_equal(elqrGaussian.safety_factor, 1.)
assert len(elqrGaussian.targets.means) == 0
with pytest.raises(RuntimeError, match=r"Horizon .* ELQR"):
elqrGaussian = guide.ELQRGaussian(horizon_len=np.inf)
def test_find_nearest_target(self, wayareas, waypoints):
elqrGaussian = guide.ELQRGaussian(Q=np.eye(4),
R=np.eye(2),
wayareas=wayareas)
state = waypoints[0] + 2
exp_goal = waypoints[0]
goal = elqrGaussian.find_nearest_target(state)
test.assert_allclose(goal, exp_goal)
def test_final_cost_function(self, wayareas, waypoints):
elqrGaussian = guide.ELQRGaussian(Q=np.eye(4),
R=np.eye(2),
wayareas=wayareas)
states = np.zeros((2, 4)) # num objects by num states
states[0, :] = waypoints[0].squeeze() + 2
states[1, :] = waypoints[1].squeeze() + 1
exp_cost = 20
cost = elqrGaussian.final_cost_function(states)
test.assert_approx_equal(cost, exp_cost)
def test_quadratize_cost(self, wayareas):
cur_states = np.array(
[[-6.61516208455517, 7.31118852531110, -0.872274587078559],
[0.120718435589312, 0.822267374847013, 8.03032533284825],
[-3.70905023923744, 0.109099775533068, 0.869292134497581],
[-0.673511914127673, 0.0704886302223785, 1.37195762193361]])
obj_num = 0
c1 = np.array([[
0.159153553514662, -0.0131131393293015, 0.0679982358692922,
-0.00259902229574857
],
[
-0.0131131393293015, 1.09141824323191,
-0.00485217347554817, 0.242777767799848
],
[
0.0679982358692923, -0.00485217347554816,
0.0524826749134647, -0.00113766933711917
],
[
-0.00259902229574857, 0.242777767799848,
-0.00113766933711917, 0.182621583380717
]])
c2 = np.array([[
0.168615126193114, 0.000427327257979604, 0.0813173846497561,
3.60271394071290e-05
],
[
0.000427327257979604, 1.63427398127019,
0.000214726953674745, 0.131506716663899
],
[
0.0813173846497561, 0.000214726953674745,
0.168683528526021, 1.81032165630712e-05
],
[
3.60271394071290e-05, 0.131506716663899,
1.81032165630712e-05, 0.240588802794357
]])
c3 = np.array([[
0.172523281175800, -0.0880334775266906, 0.0824376519837369,
-0.0127439165441394
],
[
-0.0880334775266907, 1.17602498884724,
-0.0323162663735516, 0.221483755921902
],
[
0.0824376519837369, -0.0323162663735516,
0.0792669538504277, -0.00591837975207081
],
[
-0.0127439165441394, 0.221483755921902,
-0.00591837975207081, 0.206656002000548
]])
cov = [c1, c2, c3]
cur_gaussians = []
weights = [0.326646270304980, 0.364094073944343, 0.309259655750677]
ctr1 = np.array([0.798419210129032, 0.803706982685878])
ctr2 = np.array([0., 0.])
ctr3 = np.array([-0.700669070490184, 1.52231152743820])
ctr = [ctr1, ctr2, ctr3]
for ii in range(0, 3):
gm = guide.GaussianObject()
gm.means = cur_states[:, ii].reshape((1, 4))
gm.ctrl_inputs = ctr[ii]
gm.covariance = cov[ii]
gm.weight = weights[ii]
cur_gaussians.append(gm)
for cov in wayareas.covariances:
cov[2, 2] = 100.
cov[3, 3] = 100.
wayareas.weights = [0.5, 0.5, 0.5, 0.5]
elqrGaussian = guide.ELQRGaussian(Q=np.eye(4),
R=np.eye(2),
cur_gaussians=cur_gaussians,
wayareas=wayareas,
horizon_len=3)
x_start = np.zeros((4, 1))
u_nom = np.zeros((2, 1))
exp_Q = np.array(
[[
6.08618287e-08, -7.62808745e-06, -1.31408886e-07,
-5.24016660e-10
],
[-7.62808745e-06, 9.56062598e-04, 1.64700683e-05, 6.56773710e-08],
[-1.31408886e-07, 1.64700683e-05, 2.83729487e-07, 1.13142255e-09],
[-5.24016660e-10, 6.56773710e-08, 1.13142255e-09,
4.51175171e-12]])
exp_q = np.array([[7.570267e-03], [1.793011e-05], [1.558761e-04],
[2.919833e-05]])
P, Q, R, q, r = elqrGaussian.quadratize_cost(x_start,
u_nom,
1,
all_states=cur_states,
obj_num=obj_num)
test.assert_allclose(Q, exp_Q)
test.assert_allclose(q, exp_q, atol=1e-8)
def test_initialize(self):
meas = GaussianMixture()
meas.means = [np.array([[1], [2], [3]]), np.array([[4], [5], [6]])]
meas.covariances = [2 * np.eye(3), 2 * np.eye(3)]
meas.weights = [1, 1]
dyn_lst = [[], []] # list of lists of functions
inv_dyn_lst = [[], []] # list of lists of functions
n_inputs = [1, 1]
elqrGaussian = guide.ELQRGaussian()
elqrGaussian.initialize(meas, dyn_lst, inv_dyn_lst, n_inputs)
assert len(elqrGaussian.gaussians) == len(meas.means)
for ii, gg in enumerate(elqrGaussian.gaussians):
test.assert_allclose(gg.means[[0], :].T,
meas.means[ii],
err_msg='Iteration number: {}'.format(ii))
obj = guide.GaussianObject()
obj.means = np.array([[1, 2, 3], [1, 2, 3], [1, 2, 3]]) + 0.0001
obj.covariance = np.eye(3) + 0.01
obj.weight = 1
obj.dyn_functions = []
obj.inv_dyn_functions = []
obj.ctrl_inputs = np.array([[1], [1], [1]])
obj.feedforward_lst = [
np.zeros((1, 1)),
np.zeros((1, 1)),
np.zeros((1, 1))
]
obj.feedback_lst = [
np.zeros((1, 3)),
np.zeros((1, 3)),
np.zeros((1, 3))
]
obj.cost_to_come_mat = [
np.zeros((3, 3)),
np.zeros((3, 3)),
np.zeros((3, 3))
]
obj.cost_to_come_vec = [
np.zeros((3, 1)),
np.zeros((3, 1)),
np.zeros((3, 1))
]
obj.cost_to_go_mat = [
np.zeros((3, 3)),
np.zeros((3, 3)),
np.zeros((3, 3))
]
obj.cost_to_go_vec = [
np.zeros((3, 1)),
np.zeros((3, 1)),
np.zeros((3, 1))
]
obj.ctrl_nom = np.array([[0]])
elqrGaussian = guide.ELQRGaussian(cur_gaussians=[obj])
elqrGaussian.initialize(meas, dyn_lst, inv_dyn_lst, n_inputs)
assert len(elqrGaussian.gaussians) == len(meas.means)
obj2 = deepcopy(obj)
obj2.means = np.array([[4, 5, 6], [4, 5, 6], [4, 5, 6]]) + 0.000001
elqrGaussian = guide.ELQRGaussian(cur_gaussians=[obj, obj2])
elqrGaussian.initialize(meas, dyn_lst, inv_dyn_lst, n_inputs)
for ii, gg in enumerate(elqrGaussian.gaussians):
test.assert_allclose(gg.means[[0], :].T,
meas.means[ii],
err_msg='Iteration number: {}'.format(ii))