Support other sos types. (#58)

We can use SDSOS or DSOS.
Quadrotor has better numerics when we use SDSOS.

compatible_clf_cbf/clf_cbf.py

@@ -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:
@@ -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)
@@ -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(
             [
@@ -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
                 ]
             )
@@ -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
@@ -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,
@@ -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)
 
@@ -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 = (
@@ -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],
@@ -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:
@@ -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
@@ -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)
@@ -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:
@@ -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
@@ -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(
@@ -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],
@@ -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)

examples/quadrotor/demo.py

@@ -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"