Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support other sos types. #58

Merged
merged 1 commit into from
Aug 13, 2024
Merged

Support other sos types. #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
54 changes: 46 additions & 8 deletions compatible_clf_cbf/clf_cbf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,7 @@ def construct_polynomial(
x: sym.Variables,
y: sym.Variables,
is_sos: bool,
sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> sym.Polynomial:
"""
Args:
Expand All @@ -123,7 +124,7 @@ def construct_polynomial(
basis = sym.MonomialBasis(
{x: int(np.floor(self.x / 2)), y: int(np.floor(self.y / 2))}
)
poly, _ = prog.NewSosPolynomial(basis)
poly, _ = prog.NewSosPolynomial(basis, type=sos_type)
else:
basis = sym.MonomialBasis({x: self.x, y: self.y})
coeffs = prog.NewContinuousVariables(basis.size)
Expand All @@ -138,7 +139,12 @@ def construct_polynomial(
state_eq_constraints: Optional[List[Degree]]

def to_lagrangians(
self, prog: solvers.MathematicalProgram, x: sym.Variables, y: sym.Variables
self,
prog: solvers.MathematicalProgram,
x: sym.Variables,
y: sym.Variables,
*,
sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> CompatibleLagrangians:
lambda_y = np.array(
[
Expand All @@ -151,20 +157,27 @@ def to_lagrangians(
None
if self.y is None
else np.array(
[y_i.construct_polynomial(prog, x, y, is_sos=True) for y_i in self.y]
[
y_i.construct_polynomial(prog, x, y, is_sos=True, sos_type=sos_type)
for y_i in self.y
]
)
)
rho_minus_V = (
None
if self.rho_minus_V is None
else self.rho_minus_V.construct_polynomial(prog, x, y, is_sos=True)
else self.rho_minus_V.construct_polynomial(
prog, x, y, is_sos=True, sos_type=sos_type
)
)
b_plus_eps = (
None
if self.b_plus_eps is None
else np.array(
[
b_plus_eps_i.construct_polynomial(prog, x, y, is_sos=True)
b_plus_eps_i.construct_polynomial(
prog, x, y, is_sos=True, sos_type=sos_type
)
for b_plus_eps_i in self.b_plus_eps
]
)
Expand Down Expand Up @@ -594,6 +607,8 @@ def construct_search_compatible_lagrangians(
lagrangian_degrees: CompatibleLagrangianDegrees,
barrier_eps: Optional[np.ndarray],
local_clf: bool = True,
lagrangian_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> Tuple[solvers.MathematicalProgram, CompatibleLagrangians]:
"""
Given CLF candidate V and CBF candidate b, construct the optimization
Expand All @@ -617,7 +632,9 @@ def construct_search_compatible_lagrangians(
"""
prog = solvers.MathematicalProgram()
prog.AddIndeterminates(self.xy_set)
lagrangians = lagrangian_degrees.to_lagrangians(prog, self.x_set, self.y_set)
lagrangians = lagrangian_degrees.to_lagrangians(
prog, self.x_set, self.y_set, sos_type=lagrangian_sos_type
)
self._add_compatibility(
prog=prog,
V=V,
Expand All @@ -627,6 +644,7 @@ def construct_search_compatible_lagrangians(
lagrangians=lagrangians,
barrier_eps=barrier_eps,
local_clf=local_clf,
sos_type=compatible_sos_type,
)
return (prog, lagrangians)

Expand All @@ -642,14 +660,24 @@ def search_lagrangians_given_clf_cbf(
solver_id: Optional[solvers.SolverId] = None,
solver_options: Optional[solvers.SolverOptions] = None,
lagrangian_coefficient_tol: Optional[float] = None,
lagrangian_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> Tuple[
Optional[CompatibleLagrangians], List[Optional[UnsafeRegionLagrangians]]
]:
(
prog_compatible,
compatible_lagrangians,
) = self.construct_search_compatible_lagrangians(
V, b, kappa_V, kappa_b, compatible_lagrangian_degrees, barrier_eps
V,
b,
kappa_V,
kappa_b,
compatible_lagrangian_degrees,
barrier_eps,
local_clf=True,
lagrangian_sos_type=lagrangian_sos_type,
compatible_sos_type=compatible_sos_type,
)
result_compatible = solve_with_id(prog_compatible, solver_id, solver_options)
compatible_lagrangians_result = (
Expand Down Expand Up @@ -690,6 +718,7 @@ def search_clf_cbf_given_lagrangian(
solver_options: Optional[solvers.SolverOptions] = None,
backoff_rel_scale: Optional[float] = None,
backoff_abs_scale: Optional[float] = None,
compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> Tuple[
Optional[sym.Polynomial],
Optional[np.ndarray],
Expand All @@ -713,6 +742,7 @@ def search_clf_cbf_given_lagrangian(
kappa_V,
kappa_b,
barrier_eps,
compatible_sos_type=compatible_sos_type,
)

if ellipsoid_inner is not None:
Expand Down Expand Up @@ -878,6 +908,8 @@ def bilinear_alternation(
binary_search_scale_options: Optional[BinarySearchOptions] = None,
compatible_states_options: Optional[CompatibleStatesOptions] = None,
backoff_scales: Optional[List[compatible_clf_cbf.utils.BackoffScale]] = None,
lagrangian_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> Tuple[Optional[sym.Polynomial], np.ndarray]:
"""
Synthesize the compatible CLF and CBF through bilinear alternation. We
Expand Down Expand Up @@ -953,6 +985,8 @@ def evaluate_compatible_states(clf_fun, cbf_funs, x_val):
solver_id,
solver_options,
lagrangian_coefficient_tol,
lagrangian_sos_type=lagrangian_sos_type,
compatible_sos_type=compatible_sos_type,
)
assert compatible_lagrangians is not None
assert all(unsafe_lagrangians)
Expand Down Expand Up @@ -1027,6 +1061,7 @@ def evaluate_compatible_states(clf_fun, cbf_funs, x_val):
if backoff_scales is None
else backoff_scales[iteration].abs
),
compatible_sos_type=compatible_sos_type,
)
assert cbf is not None
if compatible_states_options is not None:
Expand Down Expand Up @@ -1163,6 +1198,7 @@ def _add_compatibility(
lagrangians: CompatibleLagrangians,
barrier_eps: Optional[np.ndarray],
local_clf: bool,
sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> sym.Polynomial:
"""
Add the p-satz condition that certifies the following set is empty
Expand Down Expand Up @@ -1236,7 +1272,7 @@ def _add_compatibility(
assert lagrangians.state_eq_constraints is not None
poly -= lagrangians.state_eq_constraints.dot(self.state_eq_constraints)

prog.AddSosConstraint(poly)
prog.AddSosConstraint(poly, sos_type)
return poly

def _add_barrier_safe_constraint(
Expand Down Expand Up @@ -1293,6 +1329,7 @@ def _construct_search_clf_cbf_program(
kappa_b: np.ndarray,
barrier_eps: np.ndarray,
local_clf: bool = True,
compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos,
) -> Tuple[
solvers.MathematicalProgram,
Optional[sym.Polynomial],
Expand Down Expand Up @@ -1360,6 +1397,7 @@ def _construct_search_clf_cbf_program(
lagrangians=compatible_lagrangians,
barrier_eps=barrier_eps,
local_clf=local_clf,
sos_type=compatible_sos_type,
)

return (prog, V, b)
Expand Down
1 change: 1 addition & 0 deletions examples/quadrotor/demo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,7 @@ def main(use_y_squared: bool, with_u_bound: bool):
lagrangian_coefficient_tol=None,
compatible_states_options=compatible_states_options,
backoff_scales=backoff_scales,
lagrangian_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSdsos,
)
pickle_path = os.path.join(
os.path.dirname(os.path.abspath(__file__)), "../../data/quadrotor_clf_cbf.pkl"
Expand Down