From a1cb2c78c3dab90def0f18ccffdfe9b19636ef0f Mon Sep 17 00:00:00 2001
From: Hongkai Dai <daihongkai@gmail.com>
Date: Sun, 21 Jan 2024 11:04:56 -0800
Subject: [PATCH] Add the trigonometric dynamics to the nonlinear toy system.
 (#39)

---
 .github/workflows/python-app.yml              |  2 +
 examples/nonlinear_toy/README.md              | 26 ++++++++
 examples/nonlinear_toy/toy_system.py          | 24 +++++++
 .../wo_input_limits_trigpoly_demo.py          | 66 +++++++++++++++++++
 4 files changed, 118 insertions(+)
 create mode 100644 examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py

diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml
index 4c16957..776edc5 100644
--- a/.github/workflows/python-app.yml
+++ b/.github/workflows/python-app.yml
@@ -25,3 +25,5 @@ jobs:
         run: python examples/linear_toy/linear_toy_demo.py
       - name: run nonlinear_toy_demo
         run: python examples/nonlinear_toy/wo_input_limits_demo.py
+      - name: run nonlinear_toy_trigpoly_demo
+        run: python examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py
diff --git a/examples/nonlinear_toy/README.md b/examples/nonlinear_toy/README.md
index 368abf2..abd607b 100644
--- a/examples/nonlinear_toy/README.md
+++ b/examples/nonlinear_toy/README.md
@@ -9,3 +9,29 @@ $$
 We will consider the case with or without the input limits on $u$.
 
 This system is introduced in *Searching for control Lyapunov functions using sums-of-squares programming* by Weehong Tan and Andrew Packard.
+
+I _think_ this system is the taylor expansion of the following system
+$$
+\begin{align*}
+\dot{x}_0 = &u\\
+\dot{x}_1=&-\sin x_0 -u
+\end{align*}
+$$
+
+So another way to model this system is to use the trigonometric state
+$$
+\begin{align*}
+\bar{x}_0 =& \sin x_0\\
+\bar{x}_1 =& \cos x_0 -1\\
+\bar{x}_2 =& x_1
+\end{align*}
+$$ 
+Note that the new state $\bar{x}$ has the equilibrium $\bar{x}^*=0$.
+The dynamics is
+$$
+\begin{align*}
+\dot{\bar{x}}_0 =& (\bar{x}_1+1)u\\
+\dot{\bar{x}}_1=&-\bar{x}_0u\\
+\dot{\bar{x}}_2=&-\bar{x}_0 -u
+\end{align*}
+$$
diff --git a/examples/nonlinear_toy/toy_system.py b/examples/nonlinear_toy/toy_system.py
index 042a24d..e766ab4 100644
--- a/examples/nonlinear_toy/toy_system.py
+++ b/examples/nonlinear_toy/toy_system.py
@@ -32,3 +32,27 @@ def affine_dynamics(x: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
         g = np.array([[1], [-1]])
 
     return (f, g)
+
+
+def affine_trig_poly_state_constraints(x: np.ndarray) -> sym.Polynomial:
+    """
+    With the state x̅ = [sinx₀, cosx₀−1, x₁], the state constraint is x̅₀² + (x̅₁+1)²=1
+    """
+    return sym.Polynomial(x[0] ** 2 + x[1] ** 2 + 2 * x[1])
+
+
+def affine_trig_poly_dynamics(x: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    With the trigonometric state x̅ = [sinx₀, cosx₀−1, x₁], we can write the
+    dynamics as affine form f(x̅) + g(x̅)u
+    """
+    assert x.shape == (3,)
+    if x.dtype == object:
+        f = np.array([sym.Polynomial(), sym.Polynomial(), sym.Polynomial(-x[0])])
+        g = np.array(
+            [[sym.Polynomial(x[1] + 1)], [sym.Polynomial(-x[0])], [sym.Polynomial(-1)]]
+        )
+    else:
+        f = np.array([0, 0, -x[0]])
+        g = np.array([[x[1] + 1], [-x[0]], [-1]])
+    return f, g
diff --git a/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py b/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py
new file mode 100644
index 0000000..e6259d2
--- /dev/null
+++ b/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py
@@ -0,0 +1,66 @@
+"""
+Find the CLF/CBF without the input limits.
+We use the trigonometric state with polynomial dynamics.
+"""
+
+import numpy as np
+
+import pydrake.solvers as solvers
+import pydrake.symbolic as sym
+
+import compatible_clf_cbf.clf_cbf as clf_cbf
+import examples.nonlinear_toy.toy_system as toy_system
+
+
+def main():
+    x = sym.MakeVectorContinuousVariable(3, "x")
+    f, g = toy_system.affine_trig_poly_dynamics(x)
+    state_eq_constraints = np.array([toy_system.affine_trig_poly_state_constraints(x)])
+    use_y_squared = True
+    compatible = clf_cbf.CompatibleClfCbf(
+        f=f,
+        g=g,
+        x=x,
+        unsafe_regions=[np.array([sym.Polynomial(x[0] + x[1] + x[2] + 3)])],
+        Au=None,
+        bu=None,
+        with_clf=True,
+        use_y_squared=use_y_squared,
+        state_eq_constraints=state_eq_constraints,
+    )
+    V_init = sym.Polynomial(x[0] ** 2 + x[1] ** 2 + x[2] ** 2)
+    b_init = np.array([sym.Polynomial(0.1 - x[0] ** 2 - x[1] ** 2 - x[2] ** 2)])
+
+    lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees(
+        lambda_y=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0)],
+        xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0),
+        y=None,
+        rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2),
+        b_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)],
+        state_eq_constraints=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)],
+    )
+    rho = 0.1
+    kappa_V = 0.01
+    kappa_b = np.array([0.01])
+    barrier_eps = np.array([0.001])
+    (
+        compatible_prog,
+        compatible_lagrangians,
+    ) = compatible.construct_search_compatible_lagrangians(
+        V_init,
+        b_init,
+        kappa_V,
+        kappa_b,
+        lagrangian_degrees,
+        rho,
+        barrier_eps,
+    )
+    assert compatible_lagrangians is not None
+    solver_options = solvers.SolverOptions()
+    solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 0)
+    compatible_result = solvers.Solve(compatible_prog, None, solver_options)
+    assert compatible_result.is_success()
+
+
+if __name__ == "__main__":
+    main()