def get_k_and_p(graph, X, U):
'''Finds optimal control law given by $u=Kx$ and value function $Vx^2$ aka
cost-to-go which corresponds to solutions to the algebraic, finite
horizon Ricatti Equation. K is Extracted from the bayes net and V is
extracted by incrementally eliminating the factor graph.
Arguments:
graph: factor graph containing factor graph in LQR form
X: list of state Keys
U: list of control Keys
Returns:
K: optimal control matrix, shape (T-1, 1)
V: value function, shape (T, 1)
TODO(gerry): support n-dimensional state space
'''
T = len(X)
# Find K and V by using bayes net solution
marginalized_fg = graph
K = np.zeros((T - 1, 1))
P = np.zeros((T, 1))
P[-1] = get_return_cost(marginalized_fg, X[-1])
for i in range(len(U) - 2, -1, -1): # traverse backwards in time
ordering = gtsam.Ordering()
ordering.push_back(X[i + 1])
ordering.push_back(U[i])
bayes_net, marginalized_fg = marginalized_fg.eliminatePartialSequential(
ordering)
P[i] = get_return_cost(marginalized_fg, X[i])
K[i] = bayes_net.back().S() # note: R is 1
return K, P
def test_eliminate(self):
# Recommended way to specify a matrix (see cython/README)
Ax2 = np.array([[-5., 0.], [+0., -5.], [10., 0.], [+0., 10.]],
order='F')
# This is good too
Al1 = np.array([[5, 0], [0, 5], [0, 0], [0, 0]],
dtype=float,
order='F')
# Not recommended for performance reasons, but should still work
# as the wrapper should convert it to the correct type and storage order
Ax1 = np.array([
[0, 0], # f4
[0, 0], # f4
[-10, 0], # f2
[0, -10]
]) # f2
x2 = 1
l1 = 2
x1 = 3
# the RHS
b2 = np.array([-1., 1.5, 2., -1.])
sigmas = np.array([1., 1., 1., 1.])
model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
# eliminate the first variable (x2) in the combined factor, destructive
# !
ord = gtsam.Ordering()
ord.push_back(x2)
actualCG, lf = combined.eliminate(ord)
# create expected Conditional Gaussian
R11 = np.array([[11.1803, 0.00], [0.00, 11.1803]])
S12 = np.array([[-2.23607, 0.00], [+0.00, -2.23607]])
S13 = np.array([[-8.94427, 0.00], [+0.00, -8.94427]])
d = np.array([2.23607, -1.56525])
expectedCG = gtsam.GaussianConditional(x2, d, R11, l1, S12, x1, S13,
gtsam.noiseModel_Unit.Create(2))
# check if the result matches
self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
# the expected linear factor
Bl1 = np.array([[4.47214, 0.00], [0.00, 4.47214]])
Bx1 = np.array(
# x1
[[-4.47214, 0.00], [+0.00, -4.47214]])
# the RHS
b1 = np.array([0.0, 0.894427])
model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
# check if the result matches the combined (reduced) factor
self.gtsamAssertEquals(lf, expectedLF, 1e-4)
def test_ordering(self):
"""Test ordering"""
gfg, keys = create_graph()
ordering = gtsam.Ordering()
for key in keys[::-1]:
ordering.push_back(key)
bn = gfg.eliminateSequential(ordering)
self.assertEqual(bn.size(), 3)
keyVector = gtsam.KeyVector()
keyVector.append(keys[2])
def get_k(graph, X, U):
"""Finds the optimal control law given by $u=Kx$ but not the value function
$Vx^2$ aka cost-to-go.
Arguments:
graph: factor graph containing factor graph in LQR form
X: list of state Keys
U: list of control Keys
Returns:
K: optimal control matrix, shape (T-1, 1)
TODO(gerry): support n-dimensional state space (just change size of K)
"""
T = len(U)
K = np.zeros((T-1, 1))
ordering = gtsam.Ordering()
for i in range(T - 1, -1, -1): # traverse backwards in time
ordering.push_back(U[i])
ordering.push_back(X[i])
net = graph.eliminateSequential(ordering)
for i in range(T - 1):
cond = net.at(2 * (T - 1 - i))
K[i] = solve_triangular(cond.R(), cond.S())
return K
