Include the following components for HOCBFs in utils. (#82)

* Include the following components for HOCBFs in utils.
1. elemntary symmetric polynomials,
2. lie derivatives, and
3. lower lie derivative

* Include the following components for HOCBFs in utils.
1. elemntary symmetric polynomials,
2. lie derivatives, and
3. lower lie derivative

* Include the following components for HOCBFs in utils.
1. elemntary symmetric polynomials,
2. lie derivatives, and
3. lower lie derivative

.vscode/settings.json

@@ -0,0 +1,14 @@
+{
+    "python.testing.unittestArgs": [
+        "-v",
+        "-s",
+        "./tests",
+        "-p",
+        "test_*.py"
+    ],
+    "python.testing.pytestEnabled": true,
+    "python.testing.unittestEnabled": false,
+    "python.testing.pytestArgs": [
+        "tests"
+    ]
+}

compatible_clf_cbf/utils.py

@@ -2,11 +2,101 @@
 from typing import Dict, List, Optional, Tuple, Union
 from typing_extensions import Self
 import numpy as np
+import itertools
 
 import pydrake.symbolic as sym
 import pydrake.solvers as solvers
 
 
+def elementary_symmetric_polynomials(input: Optional[list]) -> np.ndarray:
+    """
+    given a set of numbers, compute the symetric polynomials:
+    for example, given [a,b,c]
+    the output should be [1, a+b+c, ab+bc+ac, abc]
+    this function is used for computing high relative degree weights
+    for different power of Lie derivatives.
+    """
+    output = [1.0]
+    if input is not None:
+        N = len(input)
+        for i in range(1, N + 1):
+            product_terms = []
+            for comb in itertools.combinations(input, i):
+                product_terms.append(np.prod(comb))
+            sum_of_products = sum(product_terms)
+            output.append(sum_of_products)
+    return np.array(output)
+
+
+def lie_derivative(
+    poly: sym.Polynomial, vector_feild: np.ndarray, variables: np.ndarray, pow: int
+) -> sym.Polynomial:
+    """
+    compute the n-power lie derivative of a polynomial with respect to the
+    vector feild f. The output should be Lf^nb(x), where the b(x) is the
+    input polynomial, and f(x), the vector feild, is an array of polynomials.
+    Both f(x) and b(x) are based on x variables.
+
+    Args:
+    poly: the input polynomial b(x)
+    vector_feild: the vector feild f(x), an array of polynomials
+    variables: the variables x
+    pow: the power of the lie derivative
+    """
+
+    temp_x = poly
+    if pow == 0:
+        return temp_x
+    elif pow >= 1:
+        for i in range(1, pow + 1):
+            temp_x = np.dot(temp_x.Jacobian(variables), vector_feild)
+        return temp_x
+    else:
+        assert pow >= 0, "power of lie derivative should not be negative"
+
+
+def lower_lie_derivatives(
+    poly: sym.Polynomial,
+    vector_feild: np.ndarray,
+    variables: np.ndarray,
+    relative_degree: int,
+    betas: list[float],
+) -> np.ndarray:
+    """
+    Assume relative degree = n, an HOCBF is valid if and only if:
+    ∀ x ∈ {x|b(x)≥0, Lfb(x)+β1b(x)≥0, Lf^2b(x)+(β2+β1)Lfb(x)+β1β2b(x)≥0, ...,}
+    ∃ u ∈ U such that:
+        Lf^(n-1)Lgb(x)u+
+        Lf^nb(x)+(βn+βn-1+...+β2+β1)Lf^(n-1)b(x)+
+        ...
+        +β1β2b(x)≥0,
+    this function computes an array of polynomials:
+        Lfb(x)+β1b(x)
+        Lf^2b(x)+(β2+β1)Lfb(x)+β1β2b(x)
+        ...
+    In our journal extension, we give the definition of HOCBFs by using a Phi(x) vector,
+    this function computes the elements:
+      Phi_1(x), Phi_2(x), ..., Phi_(n-1)(x) (without Phi_0(x))
+    in that vector, where n is the relative degree.
+    """
+
+    output = np.empty(relative_degree - 1, dtype=object)
+    # the range is from 1 to relative_degree, with relative_degree excluded.
+    # hence if the relative_degree is 1, the output should be an empty array.
+    for i in range(1, relative_degree):
+        beta_vector = elementary_symmetric_polynomials(betas[:i])
+        lie_derivatives = np.array(
+            [
+                lie_derivative(
+                    poly=poly, vector_feild=vector_feild, variables=variables, pow=j
+                )
+                for j in range(i, -1, -1)
+            ]
+        )
+        output[i - 1] = np.dot(beta_vector, lie_derivatives)
+    return output
+
+
 def check_array_of_polynomials(p: np.ndarray, x_set: sym.Variables) -> None:
     """
     Check if each element of p is a symbolic polynomial, whose indeterminates

tests/test_utils.py

@@ -7,6 +7,69 @@
 import pydrake.solvers as solvers
 
 
+def test_elementary_symmetric_polynomials():
+    """
+    During the project, we only pass list of float numbers to the function:
+      elementary_symmetric_polynomials().
+    But in order to test the function, we pass list of symbolic variables to
+    this function, and check whether the output
+    expresstion is correct.
+    """
+    a = sym.Variable("a")
+    b = sym.Variable("b")
+    c = sym.Variable("c")
+    input1 = []
+    input2 = [a, b, c]
+    output1 = mut.elementary_symmetric_polynomials(input1)
+    output2 = mut.elementary_symmetric_polynomials(input2)
+    expected_output1 = 1
+    expected_output2 = np.array([1, a + b + c, a * b + a * c + b * c, a * b * c])
+    assert output1 == expected_output1
+    assert output2[0] == expected_output2[0]
+    for i in range(1, output2.size):
+        assert output2[i].EqualTo(expected_output2[i])
+
+
+def test_lie_derivative():
+    x = sym.MakeVectorContinuousVariable(2, "x")
+    b1 = sym.Polynomial(x[0] + x[1])
+    b2 = sym.Polynomial(x[0] * x[1])
+    b3 = b2
+    pow1 = 0
+    pow2 = 1
+    pow3 = 2
+    f = np.array([sym.Polynomial(x[0] ** 2), sym.Polynomial(x[1] ** 2)])
+    expected_output1 = b1
+    expected_output2 = sym.Polynomial(x[1] * x[0] ** 2 + x[0] * x[1] ** 2)
+    expected_output3 = sym.Polynomial(
+        2 * x[1] * x[0] ** 3 + 2 * x[0] ** 2 * x[1] ** 2 + 2 * x[0] * x[1] ** 3
+    )
+    output1 = mut.lie_derivative(poly=b1, vector_feild=f, variables=x, pow=pow1)
+    output2 = mut.lie_derivative(poly=b2, vector_feild=f, variables=x, pow=pow2)
+    output3 = mut.lie_derivative(poly=b3, vector_feild=f, variables=x, pow=pow3)
+    assert output1.EqualTo(expected_output1)
+    assert output2.EqualTo(expected_output2)
+    assert output3.EqualTo(expected_output3)
+
+
+def test_lower_lie_drivatives():
+    x = sym.MakeVectorContinuousVariable(2, "x")
+    b = sym.Polynomial(x[0] + 1)
+    f = np.array([sym.Polynomial(x[1]), sym.Polynomial()])
+    r = 2
+    betas = [1.0, 1.0]
+    expected_ouput = np.array([sym.Polynomial(x[0] + x[1] + 1)])
+    output = mut.lower_lie_derivatives(
+        poly=b,
+        vector_feild=f,
+        variables=x,
+        relative_degree=r,
+        betas=betas,
+    )
+    assert output.size == 1
+    assert output[0].EqualTo(expected_ouput[0])
+
+
 def test_check_array_of_polynomials():
     x = sym.MakeVectorContinuousVariable(rows=3, name="x")
     x_set = sym.Variables(x)