From 4094906ee2149ba738f6463869c5d617b29433f0 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Wed, 29 Nov 2023 23:16:28 -0800 Subject: [PATCH] Add a toy 2D system with nonlinear dynamics. We can certify the CLF/CBF are compatible for this toy system. --- .github/workflows/python-app.yml | 2 + compatible_clf_cbf/clf_cbf.py | 3 +- examples/nonlinear_toy/README.md | 11 ++++ examples/nonlinear_toy/toy_system.py | 34 ++++++++++ .../nonlinear_toy/wo_input_limits_demo.py | 65 +++++++++++++++++++ 5 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 examples/nonlinear_toy/README.md create mode 100644 examples/nonlinear_toy/toy_system.py create mode 100644 examples/nonlinear_toy/wo_input_limits_demo.py diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 0582c5b..4c16957 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -23,3 +23,5 @@ jobs: run: pytest tests - name: run linear_toy_demo run: python examples/linear_toy/linear_toy_demo.py + - name: run nonlinear_toy_demo + run: python examples/nonlinear_toy/wo_input_limits_demo.py diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 8e2e5a7..34e61bf 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -341,7 +341,7 @@ def certify_cbf_unsafe_region( Certifies that the 0-superlevel set {x | bᵢ(x) >= 0} does not intersect with the unsafe region self.unsafe_regions[unsafe_region_index]. - If we denote the unsafe region as {x | q(x) <= 0}, then we impose the constraint + If we denote the unsafe region as {x | p(x) <= 0}, then we impose the constraint We impose the constraint -(1+ϕᵢ,₀(x))*bᵢ(x) +∑ⱼϕᵢ,ⱼ(x)pⱼ(x) is sos @@ -546,6 +546,7 @@ def _add_compatibility( # Compute s₄(x, y)ᵀ(b(x)+ε) if barrier_eps is not None: + assert np.all(barrier_eps >= 0) assert lagrangians.b_plus_eps is not None poly -= lagrangians.b_plus_eps.dot(barrier_eps + b) diff --git a/examples/nonlinear_toy/README.md b/examples/nonlinear_toy/README.md new file mode 100644 index 0000000..368abf2 --- /dev/null +++ b/examples/nonlinear_toy/README.md @@ -0,0 +1,11 @@ +We search for the compatible CLF and CBF function of a nonlinear dynamical system with 2 states + +$$ +\begin{align*} +\dot{x}_0 =& u\\ +\dot{x}_1 =& -x_0 + \frac{1}{6}x_0^3 - u +\end{align*} +$$ +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. diff --git a/examples/nonlinear_toy/toy_system.py b/examples/nonlinear_toy/toy_system.py new file mode 100644 index 0000000..2f180eb --- /dev/null +++ b/examples/nonlinear_toy/toy_system.py @@ -0,0 +1,34 @@ +from typing import Tuple + +import numpy as np +import pydrake.symbolic as sym + + +def affine_dynamics(x: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Compute the dynamics + ẋ₀ = u + ẋ₁ = −x₀ + 1/6x₀³−u + as ẋ = f(x) + g(x)*u + + return f and g. + """ + assert x.shape == (2,) + if x.dtype == object: + f = np.array( + [ + sym.Polynomial(), + sym.Polynomial( + { + sym.Monomial(x[0]): sym.Expression(-1), + sym.Monomial(x[0], 3): sym.Expression(-1.0 / 6.0), + } + ), + ] + ) + g = np.array([[sym.Polynomial(1)], [sym.Polynomial(-1)]]) + else: + f = np.array([0, -x[0] + x[0] ** 3 / 6]) + g = np.array([[1], [-1]]) + + return (f, g) diff --git a/examples/nonlinear_toy/wo_input_limits_demo.py b/examples/nonlinear_toy/wo_input_limits_demo.py new file mode 100644 index 0000000..6b97031 --- /dev/null +++ b/examples/nonlinear_toy/wo_input_limits_demo.py @@ -0,0 +1,65 @@ +""" +Find the compatible CLF/CBF with no input limits. +""" +import numpy as np +import pydrake.solvers as solvers +import pydrake.symbolic as sym + +from compatible_clf_cbf import clf_cbf +from examples.nonlinear_toy import toy_system + + +def main(use_y_squared: bool): + x = sym.MakeVectorContinuousVariable(2, "x") + f, g = toy_system.affine_dynamics(x) + use_y_squared = True + compatible = clf_cbf.CompatibleClfCbf( + f=f, + g=g, + x=x, + unsafe_regions=[np.array([sym.Polynomial(x[0] + 10)])], + Au=None, + bu=None, + with_clf=True, + use_y_squared=use_y_squared, + ) + V_init = sym.Polynomial(x[0] ** 2 + x[1] ** 2) + b_init = np.array([sym.Polynomial(0.001 - x[0] ** 2 - x[1] ** 2)]) + kappa_V = 1e-3 + kappa_b = np.array([kappa_V]) + + 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 + if use_y_squared + else [ + clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) + for _ in range(compatible.y.size) + ], + rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), + b_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0)], + ) + rho = 0.01 + barrier_eps = np.array([0.0001]) + ( + compatible_prog, + compatible_lagrangians, + ) = compatible.construct_search_compatible_lagrangians( + V_init, b_init, kappa_V, kappa_b, lagrangian_degrees, rho, barrier_eps + ) + compatible_result = solvers.Solve(compatible_prog) + assert compatible_result.is_success() + + compatible.certify_cbf_unsafe_region( + unsafe_region_index=0, + cbf=b_init[0], + cbf_lagrangian_degree=0, + unsafe_region_lagrangian_degrees=[0], + solver_options=None, + ) + + +if __name__ == "__main__": + main(use_y_squared=True) + main(use_y_squared=False)