def test_cbc2_quadratic_terms():
m = 2
n = 3
x_g = torch.rand(n)
u0 = torch.rand(m)
x = torch.rand(n)
dt = 0.01
cbf_gammas = [5., 5.]
Kp = [0.9, 1.5, 0.]
numSteps = 200
clf = CLFCartesian(Kp = torch.tensor(Kp))
ctrller = ControllerCLFBayesian(
PiecewiseLinearPlanner(x, x_g, numSteps, dt),
coordinate_converter = lambda x, x_g: x,
dynamics = LearnedShiftInvariantDynamics(dt = dt,
mean_dynamics = AckermannDrive()),
clf = clf,
cbfs = obstacles_at_mid_from_start_and_goal(x , x_g),
cbf_gammas = torch.tensor(cbf_gammas)
)
state = x
state_goal = x_g
t = 20
(bfe, e), (V, bfv, v), mean, var = cbc2_quadratic_terms(
lambda u: ctrller._clc(state, state_goal, u, t) * -1.0,
state, torch.rand(m))
assert to_numpy(bfe @ u0 + e) == pytest.approx(to_numpy(ctrller._clc(x, x_g, u0, t)), abs=1e-4, rel=1e-2)
def _qp_stability(self, clc, t, x, u0, extravars=None):
"""
SOCP compatible representation of condition
d clf(x) / dt + gamma * clf(x) < rho
"""
# grad_clf(x) @ f(x) + grad_clf(x) @ g(x) @ u + gamma * clf(x) < 0
# ||[0] [y_1; u] + [0]||_2 <= - [grad_clf(x) @ g(x)] u - grad_clf(x) @ ||f(x) - gamma * clf(x)
m = u0.shape[-1]
(bfe, e), (V, bfv, v), mean, var = cbc2_quadratic_terms(
lambda u: clc(t, u), x, u0)
A, bfb, bfc, d = SOCPController.convert_cbc_terms_to_socp_terms(
bfe, e, V, bfv, v, extravars)
# # We want to return in format?
# (name, (A, b, c, d))
# s.t. factor * ||A[y_1; u] + b||_2 <= c'u + d
return (bfc, d)
def _socp_safety(self, cbc2, x, u0,
safety_factor=None,
extravars=None):
"""
Var(CBC2) = Au² + b' u + c
E(CBC2) = e' u + e
"""
factor = safety_factor
m = u0.shape[-1]
(bfe, e), (V, bfv, v), mean, var = cbc2_quadratic_terms(cbc2, x, u0)
with torch.no_grad():
# [1, u] Asq [1; u]
Asq = torch.cat(
(
torch.cat((torch.tensor([[v]]), (bfv / 2).reshape(1, -1)), dim=-1),
torch.cat(((bfv / 2).reshape(-1, 1), V), dim=-1)
),
dim=-2)
# [1, u] Asq [1; u] = |L[1; u]|_2 = |[0, A] [y_1; y_2; u] + b|_2
A = torch.zeros((m + 1, m + extravars))
try:
L = torch.cholesky(Asq) # (m+1) x (m+1)
except RuntimeError as err:
if "cholesky" in str(err) and "singular" in str(err):
diag_e, V = torch.symeig(Asq, eigenvectors=True)
L = torch.max(torch.diag(diag_e),
torch.tensor(0.)).sqrt() @ V.t()
else:
raise
A[:, extravars:] = L[:, 1:]
b = L[:, 0] # (m+1)
c = torch.zeros((m+extravars,))
c[extravars:] = bfe
# # We want to return in format?
# (name, (A, b, c, d))
# s.t. factor * ||A[y_1; u] + b||_2 <= c'x + d
return (factor * A, factor * b, c, e)