diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml new file mode 100644 index 0000000..6faf446 --- /dev/null +++ b/.github/workflows/docs.yml @@ -0,0 +1,61 @@ +name: Deploy Docs + +on: + push: + branches: + - main + paths: + - "cbfpy/**.py" + - "docs/**" + - "examples/**.py" + - "pyproject.toml" + - ".github/workflows/docs.yml" + workflow_dispatch: + +permissions: + contents: read + pages: write + id-token: write + +concurrency: + group: pages + cancel-in-progress: true + +env: + PYTHON_VERSION: "3.10" + +jobs: + build: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python ${{ env.PYTHON_VERSION }} + uses: actions/setup-python@v5 + with: + python-version: ${{ env.PYTHON_VERSION }} + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install ".[dev]" + + - name: Build docs + run: sphinx-build -W docs/source docs/build + + - name: Upload artifact + uses: actions/upload-pages-artifact@v3 + with: + path: docs/build + + deploy: + needs: build + runs-on: ubuntu-latest + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v4 diff --git a/.github/workflows/pr-check.yml b/.github/workflows/pr-check.yml index 532feae..462cfdc 100644 --- a/.github/workflows/pr-check.yml +++ b/.github/workflows/pr-check.yml @@ -6,68 +6,75 @@ on: - main paths: - "cbfpy/**.py" + - "docs/**" - "examples/**.py" - "tests/**.py" - - "poetry.lock" + - "pyproject.toml" - ".github/workflows/pr-check.yml" env: - PYTHON_VERSION: 3.8 + PYTHON_VERSION: "3.10" jobs: test: runs-on: ubuntu-latest steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Set up Python ${{ env.PYTHON_VERSION }} - uses: actions/setup-python@v3 + uses: actions/setup-python@v5 with: python-version: ${{ env.PYTHON_VERSION }} - - name: Cache Poetry cache - uses: actions/cache@v3 - id: poetry_cache - with: - path: ~/.cache/pypoetry - key: poetry-cache-${{ env.PYTHON_VERSION }}-${{ hashFiles('**/poetry.lock') }} + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install ".[test]" + + - name: Run tests + run: python -m pytest + + lint: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 - - name: Cache Packages - uses: actions/cache@v3 - id: package_cache + - name: Set up Python ${{ env.PYTHON_VERSION }} + uses: actions/setup-python@v5 with: - path: ~/.local - key: poetry-${{ env.PYTHON_VERSION }}-${{ hashFiles('**/poetry.lock') }} + python-version: ${{ env.PYTHON_VERSION }} - - name: Install Poetry - run: curl -sSL https://install.python-poetry.org | python3 - + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install ".[dev]" - - name: Install Dependencies - if: steps.poetry_cache.outputs.cache-hit != 'true' && steps.package_cache.outputs.cache-hit != 'true' - run: poetry install + - name: Ruff check + run: ruff check . - - name: Python Test - run: poetry run task test + - name: Ruff format check + run: ruff format --check . - lint: + - name: Mypy + run: mypy . + + docs: runs-on: ubuntu-latest - needs: [test] steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - - name: Cache Poetry cache - uses: actions/cache@v3 + - name: Set up Python ${{ env.PYTHON_VERSION }} + uses: actions/setup-python@v5 with: - path: ~/.cache/pypoetry - key: poetry-cache-${{ env.PYTHON_VERSION }}-${{ hashFiles('**/poetry.lock') }} + python-version: ${{ env.PYTHON_VERSION }} - - name: Cache Packages - uses: actions/cache@v3 - with: - path: ~/.local - key: poetry-${{ env.PYTHON_VERSION }}-${{ hashFiles('**/poetry.lock') }} + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install ".[dev]" - - name: Python Lint - run: poetry run task lint + - name: Build docs + run: sphinx-build -W docs/source docs/build diff --git a/.gitignore b/.gitignore index 21d7c85..314f1f4 100644 --- a/.gitignore +++ b/.gitignore @@ -1,13 +1,21 @@ -*__pycache__ +__pycache__/ *.pyc -.mypy_cache -.pytest_cache -/docs/source/* -/docs/build/* -!/docs/source/conf.py +*.egg-info/ +dist/ +.venv/ + +# type checker / linter +.mypy_cache/ +.ruff_cache/ + +# test +.pytest_cache/ .coverage coverage.xml -cov_html -.ruff_cache -.venv -dist +cov_html/ + +# docs +/docs/build/ +/docs/source/_build/ +/docs/source/Makefile +/docs/source/make.bat diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 969e91d..0d72c0c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v5.0.0 hooks: - id: trailing-whitespace args: [--markdown-linebreak-ext=md] @@ -8,19 +8,12 @@ repos: - id: mixed-line-ending args: [--fix=lf] - id: check-added-large-files - args: ["--maxkb=10000"] # 10MB + args: ["--maxkb=10000"] - id: check-toml - id: check-yaml - - repo: https://github.com/pycqa/isort - rev: 5.11.4 - hooks: - - id: isort - - repo: https://github.com/psf/black - rev: 22.12.0 - hooks: - - id: black - language_version: python3 - - repo: https://github.com/charliermarsh/ruff-pre-commit - rev: "v0.0.221" + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.9.10 hooks: - id: ruff + args: [--fix] + - id: ruff-format diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 7e39a34..b799146 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,7 +1,8 @@ { "recommendations": [ "ms-python.python", - "njpwerner.autodocstring", + "charliermarsh.ruff", + "ms-python.mypy-type-checker", "ryanluker.vscode-coverage-gutters" ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index f33d736..6519246 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,9 +1,13 @@ { - "python.linting.mypyArgs": [ - "--config-file=./pyproject.toml" - ], "python.testing.pytestEnabled": true, "python.testing.autoTestDiscoverOnSaveEnabled": true, + "[python]": { + "editor.defaultFormatter": "charliermarsh.ruff", + "editor.formatOnSave": true, + "editor.codeActionsOnSave": { + "source.organizeImports.ruff": "explicit" + } + }, "coverage-gutters.showGutterCoverage": true, "coverage-gutters.showLineCoverage": true, "coverage-gutters.showRulerCoverage": true diff --git a/README.md b/README.md index 9bad4b1..5c91be8 100644 --- a/README.md +++ b/README.md @@ -1,19 +1,19 @@ # cbfpy Python package for using simple control barrier function. +[**Documentation**](https://toshi67026.github.io/cbfpy/) + ## Requirements -- poetry 1.3.1 +- Python >= 3.10 ## Installation -Create virtualenv and install dependencies defined for the project. ```sh -poetry install +pip install -e . ``` -Build and install locally with pip. +With dev/test dependencies: ```sh -poetry build -python -m pip install cbfpy --find-links=dist +pip install -e ".[dev,test]" ``` ## Examples @@ -24,49 +24,37 @@ python -m pip install cbfpy --find-links=dist ### Usage ```sh -poetry run python examples/example_{cbf name}.py +python examples/example_{cbf name}.py ``` -## Document -Generate document from docstring. -```sh -poetry run task docs -``` +## Documentation +Documentation is automatically published to [GitHub Pages](https://toshi67026.github.io/cbfpy/) on push to main. -Browse generated document by opening the html files in docs/build/ from your browser. - - -## Tools -### Format -- isort -- black +To build locally: ```sh -poetry run task fmt +pip install -e ".[dev]" +sphinx-build docs/source docs/build ``` +Then open `docs/build/index.html`. -### Lint -- black -- ruff -- mypy +## Tools +### Format & Lint +- [ruff](https://docs.astral.sh/ruff/) (format + lint + import sort) +- [mypy](https://mypy-lang.org/) (type check) ```sh -poetry run task lint +ruff format . +ruff check --fix . +mypy . ``` ### Test -- pytest -- pytest-cov ```sh -poetry run task test +pytest ``` The vscode extension [coverage-gutters](https://marketplace.visualstudio.com/items?itemName=ryanluker.vscode-coverage-gutters) can be used to display the test coverage. ### pre-commit Apply [config file](.pre-commit-config.yaml) for pre-commit. ```sh -poetry run pre-commit install -``` - -### Export requirements.txt -```sh -poetry export -f requirements.txt --output requirements.txt --without-hashes +pre-commit install ``` diff --git a/cbfpy/__init__.py b/cbfpy/__init__.py index e69de29..1dfb432 100755 --- a/cbfpy/__init__.py +++ b/cbfpy/__init__.py @@ -0,0 +1,28 @@ +from cbfpy.cbf import ( + CBFBase, + CircleCBF, + GeneralCBF, + LiDARCBF, + Pnorm2dCBF, + ScalarCBF, + ScalarRangeCBF, + UnicycleCircleCBF, + UnicyclePnorm2dCBF, + rotation_matrix_2d, +) +from cbfpy.cbf_qp_solver import CBFNomQPSolver, CBFQPSolver + +__all__ = [ + "CBFBase", + "CBFNomQPSolver", + "CBFQPSolver", + "CircleCBF", + "GeneralCBF", + "LiDARCBF", + "Pnorm2dCBF", + "ScalarCBF", + "ScalarRangeCBF", + "UnicycleCircleCBF", + "UnicyclePnorm2dCBF", + "rotation_matrix_2d", +] diff --git a/cbfpy/cbf.py b/cbfpy/cbf.py index 8d5ef4b..04c1175 100755 --- a/cbfpy/cbf.py +++ b/cbfpy/cbf.py @@ -1,87 +1,100 @@ #!/usr/bin/env python -from dataclasses import dataclass -from typing import Tuple, cast +from typing import cast import numpy as np from numpy.typing import NDArray from sympy import Matrix, Symbol, cos, lambdify, sin, sqrt, symbols -@dataclass +def rotation_matrix_2d(rad: float) -> NDArray: + """Create a 2D rotation matrix. + + Args: + rad: rotation angle in radians + + Returns: + 2x2 rotation matrix + """ + return np.array( + [ + [np.cos(rad), -np.sin(rad)], + [np.sin(rad), np.cos(rad)], + ] + ) + + class CBFBase: """CBF base class - Attributes: - G (NDArray): constraint matrix(=dh/dx) - h (float): constraint value(=h(x)) + The CBF optimization problem is formulated as: + minimize_{u} {objective function} + subject to G*u + alpha(h) >= 0 - Note: - The CBF optimization problem is formulated as: - minimize_{u} {objective function} - subject to G*u + alpha(h) >= 0 + Attributes: + G: constraint matrix (=dh/dx) + h: constraint value (=h(x)) """ G: NDArray h: float - def get_constraints(self) -> Tuple[NDArray, float]: + def get_constraints(self) -> tuple[NDArray, float]: """ Returns: - (NDArray): G - (NDArray): alpha(h) - - Note: - Call get_constraints individually to apply alpha function. + G: constraint matrix + alpha(h): value of alpha function applied to h """ return self.G, self._alpha(self.h) def _alpha(self, h: float) -> float: - """ + """Extended class-K function. Override to customize. + Args: - h (float): constraint value(=h(x)) + h: constraint value (=h(x)) Returns: - (float): h - - Note: - If you use specific alpha function, implement it with override. + alpha(h). Default implementation returns h itself. """ return h class GeneralCBF(CBFBase): - """ + """General-purpose CBF with manually specified constraints. + Attributes: - G (NDArray): constraint matrix(=dh/dx). shape=(1,) - h (float): constraint value(=h(x)) + G: constraint matrix (=dh/dx). shape=(N,) + h: constraint value (=h(x)) """ + def __init__(self, G: NDArray, h: float) -> None: + self.G = G + self.h = h + def calc_constraints(self, G: NDArray, h: float) -> None: self.G = G self.h = h class ScalarCBF(CBFBase): - """ + """CBF for scalar state variable. + + If keep_upper is True, the safety set is x - limit >= 0. + Attributes: - limit (float): limit for scalar state variable - keep_upper (bool): flag to prohibit going lower of the limit. Defaults to True. - x (Symbol): scalar state variable in symbolic form for cbf - sign (Symbol): symbolic variable for cbf - G (NDArray): constraint matrix(=dh/dx). shape=(1,) - h (NDArray): constraint value(=h(x)). shape=(1,) - - Note: - If keep_upper is True, the safety set is x - limit >= 0. + limit: limit for scalar state variable + keep_upper: flag to prohibit going lower of the limit + G: constraint matrix (=dh/dx). shape=(1,) + h: constraint value (=h(x)) """ - def __init__(self) -> None: - self.x = Symbol("x", real=True) # type: ignore - self.sign = Symbol("sign_", real=True) # type: ignore + def __init__(self, limit: float, keep_upper: bool = True) -> None: + self.x = Symbol("x", real=True) + self.sign = Symbol("sign_", real=True) + self.set_parameters(limit, keep_upper) def set_parameters(self, limit: float, keep_upper: bool = True) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" self.limit = limit self.keep_upper = keep_upper @@ -89,7 +102,7 @@ def set_parameters(self, limit: float, keep_upper: bool = True) -> None: self._calc_dhdx = lambdify([self.x, self.sign], cbf.diff(self.x)) self._calc_h = lambdify([self.x, self.sign], cbf) - def get_parameters(self) -> Tuple[float, bool]: + def get_parameters(self) -> tuple[float, bool]: return self.limit, self.keep_upper def calc_constraints(self, curr_value: float) -> None: @@ -100,26 +113,25 @@ def calc_constraints(self, curr_value: float) -> None: class ScalarRangeCBF(CBFBase): - """ + """CBF for scalar state variable with range constraint. + + If keep_inside is True, the safety set is a <= x <= b. + Attributes: - a (float): lower limit for scalar state variable - b (float): upper limit for scalar state variable - keep_inside (bool): flag to prohibit going outside of the range. Defaults to True. - x (Symbol): scalar state variable in symbolic form for cbf - sign (Symbol): symbolic variable for cbf - G (NDArray): constraint matrix(=dh/dx). shape=(1,) - h (float): constraint value(=h(x)) - - Note: - If keep_inside is True, the safety set is a <= x <= b. + a: lower limit for scalar state variable + b: upper limit for scalar state variable + keep_inside: flag to prohibit going outside of the range + G: constraint matrix (=dh/dx). shape=(1,) + h: constraint value (=h(x)) """ - def __init__(self) -> None: - self.x = Symbol("x", real=True) # type: ignore - self.sign = Symbol("sign_", real=True) # type: ignore + def __init__(self, a: float, b: float, keep_inside: bool = True) -> None: + self.x = Symbol("x", real=True) + self.sign = Symbol("sign_", real=True) + self.set_parameters(a, b, keep_inside) def set_parameters(self, a: float, b: float, keep_inside: bool = True) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" assert a < b self.a = a self.b = b @@ -129,7 +141,7 @@ def set_parameters(self, a: float, b: float, keep_inside: bool = True) -> None: self._calc_dhdx = lambdify([self.x, self.sign], cbf.diff(self.x)) self._calc_h = lambdify([self.x, self.sign], cbf) - def get_parameters(self) -> Tuple[float, float, bool]: + def get_parameters(self) -> tuple[float, float, bool]: return self.a, self.b, self.keep_inside def calc_constraints(self, curr_value: float) -> None: @@ -140,42 +152,42 @@ def calc_constraints(self, curr_value: float) -> None: class CircleCBF(CBFBase): - """ - Atrributes: - center (NDArray): center of circular area in world coordinate. shape=(2,) - radius (float): radius of the circular area. - keep_inside (bool): flag to prohibit going outside of the area. Defaults to True. - x (Matrix): state variables in symbolic form for cbf - sign (Symbol): symbolic variable for cbf - G (NDArray): constraint matrix(=dh/dx). shape=(2,) - h (float): constraint value(=h(x)) + """CBF for circular area constraint. + + Attributes: + center: center of circular area in world coordinate. shape=(2,) + radius: radius of the circular area + keep_inside: flag to prohibit going outside of the area + G: constraint matrix (=dh/dx). shape=(2,) + h: constraint value (=h(x)) """ - def __init__(self) -> None: - self.x = Matrix(symbols("x, y", real=True)) # type: ignore - self.sign = Symbol("sign_", real=True) # type: ignore + def __init__(self, center: NDArray, radius: float, keep_inside: bool = True) -> None: + self.x = Matrix(symbols("x, y", real=True)) + self.sign = Symbol("sign_", real=True) + self.set_parameters(center, radius, keep_inside) def set_parameters(self, center: NDArray, radius: float, keep_inside: bool = True) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" self.center = center.flatten() assert radius > 0 self.radius = radius self.keep_inside = keep_inside - cbf = self.sign * (1.0 - self.x.norm(ord=2)) # type: ignore + cbf = self.sign * (1.0 - self.x.norm(ord=2)) self._calc_dhdx = lambdify([self.x, self.sign], cbf.diff(self.x)) self._calc_h = lambdify([self.x, self.sign], cbf) - def get_parameters(self) -> Tuple[NDArray, float, bool]: + def get_parameters(self) -> tuple[NDArray, float, bool]: return self.center, self.radius, self.keep_inside def calc_constraints(self, agent_position: NDArray) -> None: """ Args: - agent_position (NDArray): agent position in world coordinate. shape=(2,) + agent_position: agent position in world coordinate. shape=(2,) Note: - division by radius is a remnant of the transformation + Division by radius is a remnant of the transformation. """ agent_position_transformed = self._transform_agent_position(agent_position.flatten()) sign = 1 if self.keep_inside else -1 @@ -185,23 +197,35 @@ def calc_constraints(self, agent_position: NDArray) -> None: self.h = self._calc_h(agent_position_transformed, sign) def _transform_agent_position(self, agent_position: NDArray) -> NDArray: - """ + """Transform agent position from world coordinate to unit circle. + Args: - agent_position (NDArray): agent position in world coordinate. shape=(2,) + agent_position: agent position in world coordinate. shape=(2,) Returns: - (NDArray): transformed agent position within unit circle. shape=(2,) + Transformed agent position within unit circle. shape=(2,) """ return cast(NDArray, ((agent_position - self.center) / self.radius)) class UnicycleCircleCBF(CircleCBF): - def __init__(self) -> None: - self.x = Matrix(symbols("x, y, theta", real=True)) # type: ignore - self.sign = Symbol("sign_", real=True) # type: ignore + """CBF for circular area constraint with unicycle model. + + Attributes: + center: center of circular area in world coordinate. shape=(2,) + radius: radius of the circular area + keep_inside: flag to prohibit going outside of the area + G: constraint matrix (=dh/dx). shape=(2,) + h: constraint value (=h(x)) + """ + + def __init__(self, center: NDArray, radius: float, keep_inside: bool = True) -> None: + self.x = Matrix(symbols("x, y, theta", real=True)) + self.sign = Symbol("sign_", real=True) + self.set_parameters(center, radius, keep_inside) def set_parameters(self, center: NDArray, radius: float, keep_inside: bool = True) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" self.center = center.flatten() assert radius > 0 self.radius = radius @@ -214,16 +238,15 @@ def set_parameters(self, center: NDArray, radius: float, keep_inside: bool = Tru def calc_constraints(self, agent_pose: NDArray) -> None: """ Args: - agent_position (NDArray): agent position in world coordinate. shape=(2,) + agent_pose: agent pose in world coordinate. shape=(3,) [x, y, theta] Note: - division by radius is a remnant of the transformation + Division by radius is a remnant of the transformation. """ sign = 1 if self.keep_inside else -1 agent_pose_transformed = self._transform_agent_pose(agent_pose.flatten()) theta = agent_pose[2] - # system matrix A = np.array( [ [np.cos(theta), 0], @@ -236,40 +259,43 @@ def calc_constraints(self, agent_pose: NDArray) -> None: self.h = self._calc_h(agent_pose_transformed, sign) def _transform_agent_pose(self, agent_pose: NDArray) -> NDArray: - """ + """Transform agent pose from world coordinate to unit circle. + Args: - agent_pose (NDArray): agent pose in world coordinate. shape=(3,) + agent_pose: agent pose in world coordinate. shape=(3,) Returns: - (NDArray): transformed agent pose within unit circle. shape=(3,) + Transformed agent pose within unit circle. shape=(3,) """ agent_position = agent_pose[0:2] - return cast(NDArray, np.append((agent_position - self.center) / self.radius, agent_pose[2])) + return np.append((agent_position - self.center) / self.radius, agent_pose[2]) class Pnorm2dCBF(CBFBase): - """ - Atrributes: - center (NDArray): center of area in world coordinate. shape=(2,) - width (NDArray): half length of the major and minor axis of ellipse + """CBF for p-norm shaped area constraint. + + Attributes: + center: center of area in world coordinate. shape=(2,) + width: half length of the major and minor axis of ellipse that match the x and y axis in the area coordinate. shape=(2,) - theta (float): rotation angle(rad) world to the area coordinate. - p (float): multiplier for p-norm. - keep_inside (bool): flag to prohibit going outside of the area. Defaults to True. - x (Matrix): state variables in symbolic form for cbf - sign (Symbol): symbolic variable for cbf - G (NDArray): constraint matrix(=dh/dx). shape=(2,) - h (float): constraint value(=h(x)) + theta: rotation angle (rad) from world to area coordinate + p: multiplier for p-norm + keep_inside: flag to prohibit going outside of the area + G: constraint matrix (=dh/dx). shape=(2,) + h: constraint value (=h(x)) """ - def __init__(self) -> None: - self.x = Matrix(symbols("x, y", real=True)) # type: ignore - self.sign = Symbol("sign_", real=True) # type: ignore + def __init__( + self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True + ) -> None: + self.x = Matrix(symbols("x, y", real=True)) + self.sign = Symbol("sign_", real=True) + self.set_parameters(center, width, theta, p, keep_inside) def set_parameters( self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True ) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" self.center = center.flatten() self.width = width.flatten() self.theta = theta @@ -277,66 +303,69 @@ def set_parameters( self.p = p self.keep_inside = keep_inside - # applyfunc(lambda x: x**self.p): element-wise power - cbf = self.sign * (1.0 - sum(abs(self.x.applyfunc(lambda x: x**self.p))) ** (1 / self.p)) # type: ignore + cbf = self.sign * (1.0 - sum(abs(self.x.applyfunc(lambda x: x**self.p))) ** (1 / self.p)) self._calc_dhdx = lambdify([self.x, self.sign], cbf.diff(self.x)) self._calc_h = lambdify([self.x, self.sign], cbf) - def get_parameters(self) -> Tuple[NDArray, NDArray, float, float, bool]: - """ - Returns: - Tuple[NDArray, NDArray, float, float, bool]: parameters - """ + def get_parameters(self) -> tuple[NDArray, NDArray, float, float, bool]: return self.center, self.width, self.theta, self.p, self.keep_inside def calc_constraints(self, agent_position: NDArray) -> None: """ Args: - agent_position (NDArray): agent position in world coordinate. shape=(2,) + agent_position: agent position in world coordinate. shape=(2,) Note: - division by width is a remnant of the transformation + Division by width is a remnant of the transformation. """ agent_position_transformed = self._transform_agent_position(agent_position.flatten()) sign = 1 if self.keep_inside else -1 - rotation_matrix = self._get_rotation_matrix(self.theta) self.G = ( - rotation_matrix @ self._calc_dhdx(agent_position_transformed, sign) / self.width.reshape(2, 1) + rotation_matrix_2d(self.theta) + @ self._calc_dhdx(agent_position_transformed, sign) + / self.width.reshape(2, 1) ).flatten() assert self.G.shape == (2,) self.h = self._calc_h(agent_position_transformed, sign) def _transform_agent_position(self, agent_position: NDArray) -> NDArray: - """ + """Transform agent position from world coordinate to normalized area coordinate. + Args: - agent_position (NDArray): agent position in world coordinate. shape=(2,) + agent_position: agent position in world coordinate. shape=(2,) Returns: - (NDArray): transformed agent position within unit circle. shape=(2,) + Transformed agent position within unit shape. shape=(2,) """ - rotation_matrix = self._get_rotation_matrix(-self.theta) - return cast(NDArray, rotation_matrix @ (agent_position - self.center) / self.width) - - @staticmethod - def _get_rotation_matrix(rad: float) -> NDArray: - return np.array( - [ - [np.cos(rad), -np.sin(rad)], - [np.sin(rad), np.cos(rad)], - ] - ) + return cast(NDArray, rotation_matrix_2d(-self.theta) @ (agent_position - self.center) / self.width) class UnicyclePnorm2dCBF(Pnorm2dCBF): - def __init__(self) -> None: - self.x = Matrix(symbols("x, y, agent_theta", real=True)) # type: ignore - self.sign = Symbol("sign_", real=True) # type: ignore + """CBF for p-norm shaped area constraint with unicycle model. + + Attributes: + center: center of area in world coordinate. shape=(2,) + width: half length of the major and minor axis of ellipse + that match the x and y axis in the area coordinate. shape=(2,) + theta: rotation angle (rad) from world to area coordinate + p: multiplier for p-norm + keep_inside: flag to prohibit going outside of the area + G: constraint matrix (=dh/dx). shape=(2,) + h: constraint value (=h(x)) + """ + + def __init__( + self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True + ) -> None: + self.x = Matrix(symbols("x, y, agent_theta", real=True)) + self.sign = Symbol("sign_", real=True) + self.set_parameters(center, width, theta, p, keep_inside) def set_parameters( self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True ) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" self.center = center.flatten() self.width = width.flatten() self.theta = theta @@ -344,7 +373,6 @@ def set_parameters( self.p = p self.keep_inside = keep_inside - # applyfunc(lambda x: x**self.p): element-wise power cbf = self.sign * ( 1.0 - sum(abs((np.hstack([np.eye(2), np.zeros([2, 1])]) @ self.x).applyfunc(lambda x: x**self.p))) @@ -356,17 +384,15 @@ def set_parameters( def calc_constraints(self, agent_pose: NDArray) -> None: """ Args: - agent_position (NDArray): agent position in world coordinate. shape=(2,) + agent_pose: agent pose in world coordinate. shape=(3,) [x, y, theta] Note: - division by radius is a remnant of the transformation + Division by width is a remnant of the transformation. """ agent_pose_transformed = self._transform_agent_pose(agent_pose.flatten()) sign = 1 if self.keep_inside else -1 - rotation_matrix = self._get_rotation_matrix(self.theta) agent_theta = agent_pose[2] - # system matrix A = np.array( [ [np.cos(agent_theta), 0], @@ -376,51 +402,48 @@ def calc_constraints(self, agent_pose: NDArray) -> None: ) dhdx = self._calc_dhdx(agent_pose_transformed, sign) assert dhdx.shape == (3, 1), dhdx - # assert False, f"{rotation_matrix},\n {dhdx},\n {dhdx[0:2]}" - # assert ( - # False - # ), f"{np.vstack([rotation_matrix @ dhdx[0:2] / self.width.reshape(2, 1), dhdx[2]]).reshape(1, -1) @ A}" self.G = ( - np.vstack([rotation_matrix @ dhdx[0:2] / self.width.reshape(2, 1), dhdx[2]]).reshape(1, -1) @ A + np.vstack([rotation_matrix_2d(self.theta) @ dhdx[0:2] / self.width.reshape(2, 1), dhdx[2]]).reshape(1, -1) + @ A ).flatten() assert self.G.shape == (2,) self.h = self._calc_h(agent_pose_transformed, sign) def _transform_agent_pose(self, agent_pose: NDArray) -> NDArray: - """ + """Transform agent pose from world coordinate to normalized area coordinate. + Args: - agent_pose (NDArray): agent pose in world coordinate. shape=(3,) + agent_pose: agent pose in world coordinate. shape=(3,) Returns: - (NDArray): transformed agent pose within unit circle. shape=(3,) + Transformed agent pose within unit shape. shape=(3,) """ - rotation_matrix = self._get_rotation_matrix(-self.theta) agent_position = agent_pose[0:2] - return cast(NDArray, np.append(rotation_matrix @ (agent_position - self.center) / self.width, agent_pose[2])) + return np.append(rotation_matrix_2d(-self.theta) @ (agent_position - self.center) / self.width, agent_pose[2]) class LiDARCBF(CBFBase): - """ + """CBF for LiDAR-based obstacle avoidance. + Attributes: - width (NDArray): half length of the major and minor axis of ellipse + width: half length of the major and minor axis of ellipse that match the x and y axis in the agent coordinate. shape=(2,) - keep_upper (bool): flag to prohibit going lower of the limit. Defaults to True. - r: state variable in symbolic form for cbf - theta: state variable in symbolic form for cbf - G (NDArray): constraint matrix(=dh/dx). shape=(2,) - h (float): constraint value(=h(x)) + keep_upper: flag to prohibit going lower of the limit + G: constraint matrix (=dh/dx). shape=(2,) + h: constraint value (=h(x)) """ - def __init__(self) -> None: - self.r = Symbol("r", real=True) # type: ignore - self.theta = Symbol("theta", real=True) # type: ignore + def __init__(self, width: NDArray, keep_upper: bool = True) -> None: + self.r = Symbol("r", real=True) + self.theta = Symbol("theta", real=True) + self.set_parameters(width, keep_upper) def set_parameters(self, width: NDArray, keep_upper: bool = True) -> None: - """Set parameters and auxiliary functions for constraint calculation""" + """Set parameters and rebuild symbolic functions.""" self.width = width.flatten() self.keep_upper = keep_upper - r_c = sqrt(sum((self.width * np.array([cos(self.theta), sin(self.theta)])) ** 2)) # type: ignore + r_c = sqrt(sum((self.width * np.array([cos(self.theta), sin(self.theta)])) ** 2)) cbf = self.r - r_c self._calc_dhdx = lambdify( [self.r, self.theta], @@ -428,7 +451,7 @@ def set_parameters(self, width: NDArray, keep_upper: bool = True) -> None: ) self._calc_h = lambdify([self.r, self.theta], cbf) - def get_parameters(self) -> Tuple[NDArray, bool]: + def get_parameters(self) -> tuple[NDArray, bool]: return self.width, self.keep_upper def calc_constraints(self, r: float, theta: float) -> None: diff --git a/cbfpy/cbf_qp_solver.py b/cbfpy/cbf_qp_solver.py index c338763..7d8b304 100755 --- a/cbfpy/cbf_qp_solver.py +++ b/cbfpy/cbf_qp_solver.py @@ -1,144 +1,115 @@ #!/usr/bin/env python -from typing import Any, List, Tuple -import cvxopt +from collections.abc import Sequence + import numpy as np +import quadprog from numpy.typing import NDArray -class QPSolver: - def __init__(self) -> None: - self._set_solver_options() +def _assemble_constraints(G_list: Sequence[NDArray]) -> NDArray: + """Assemble constraint matrix from a list of constraint vectors. - def _set_solver_options( - self, - show_progress: bool = False, - maxiters: int = 100, - abstol: float = 1e1, - reltol: float = 1e1, - feastol: float = 1e-7, - refinement: int = 0, - ) -> None: - """ - Args: - show_progress (bool, optional): Turns the output to the screen on or off. Defaults to False. - maxiters (int, optional): Maximum number of iterations. Defaults to 100. - abstol (float, optional): Absolute accuracy. Defaults to 1e1. - reltol (float, optional): Relative accuracy. Defaults to 1e1. - feastol (float, optional): Tolerance for feasibility conditions. Defaults to 1e-7. - refinement (int, optional): - Number of iterative refinement steps when solving KKT equations - (default: 0 if the problem has no second-order cone or matrix inequality constraints; 1 otherwise).. - Defaults to 0. - """ - cvxopt.solvers.options["show_progress"] = show_progress - cvxopt.solvers.options["maxiters"] = maxiters - cvxopt.solvers.options["abstol"] = abstol - cvxopt.solvers.options["reltol"] = reltol - cvxopt.solvers.options["feastol"] = feastol - cvxopt.solvers.options["refinement"] = refinement - - def _set_qp_solvers(self, P: NDArray, q: NDArray, G: NDArray, alpha_h: NDArray) -> Any: - """ - Args: - P (NDArray): optimization weight matrix. shape=(N, N) - q (NDArray): optimization weight vector. shape=(N, 1) - G (NDArray): constraint matrix. shape=(M, N) - alpha_h (NDArray): constraint vector. shape=(M,) + Args: + G_list: list of constraint vectors. [(N,), (N,), ...] - Returns: - Dict[str, List[Any]]: cvxopt.solvers.coneqp - """ - P_mat = cvxopt.matrix(P.astype("float")) - q_mat = cvxopt.matrix(q.astype("float")) - G_mat = cvxopt.matrix(G.astype("float")) - alpha_h_mat = cvxopt.matrix(alpha_h.astype("float")) + Returns: + Assembled constraint matrix. shape=(M, N) + """ + if len(G_list) > 1: + return np.array([g.flatten() for g in G_list]) + return G_list[0].reshape(1, -1) + + +def _solve_qp(P: NDArray, q: NDArray, G_ineq: NDArray, h_ineq: NDArray) -> NDArray: + """Solve a quadratic program. + + minimize_{x} (1/2) * x^T P x + q^T x + subject to G_ineq @ x <= h_ineq - try: - return cvxopt.solvers.coneqp(P_mat, q_mat, G_mat, alpha_h_mat) - except Exception as e: - raise e + Uses quadprog (Goldfarb/Idnani dual algorithm) internally. + Args: + P: positive definite weight matrix. shape=(N, N) + q: linear cost vector. shape=(N,) or (N, 1) + G_ineq: inequality constraint matrix. shape=(M, N) + h_ineq: inequality constraint bound. shape=(M,) + + Returns: + Optimal solution x. shape=(N,) + """ + # quadprog solves: min 1/2 x^T G x - a^T x, s.t. C^T x >= b + # Mapping: G=P, a=-q, C^T=-G_ineq (so C=-G_ineq^T), b=-h_ineq + x, *_ = quadprog.solve_qp( + P.astype(float), + -q.flatten().astype(float), + -G_ineq.T.astype(float), + -h_ineq.flatten().astype(float), + ) + return np.asarray(x) + + +class CBFQPSolver: + """CBF-QP solver for general quadratic cost.""" -class CBFQPSolver(QPSolver): def optimize( self, P: NDArray, q: NDArray, - G_list: List[NDArray], - alpha_h_list: List[float], - ) -> Tuple[str, NDArray]: - """ - Solve the following optimization problem + G_list: Sequence[NDArray], + alpha_h_list: list[float], + ) -> tuple[str, NDArray]: + """Solve the CBF-QP optimization problem. + minimize_{u} (1/2) * u^T*P*u + q^T*u - subject to G*u + alpha(h) >= 0 + subject to G*u <= alpha(h) Args: - P (NDArray): optimization weight matrix. shape=(N, N) - q (NDArray): optimization weight vector. shape=(N, 1) - G (List[NDArray]): constraint matrix list. [(N,), (N,), ...] - alpha_h (List[float]): constraint value list - - Notes: - cvxopt.matrix()にshapeが(N,)のNDArrayを渡すと，shapeが(N, 1)のmatrixを返す． - ソルバーに与えたいGの構造は(1, N)であるから，matrixにarrayを渡す際に(1, N)のshapeを持つようにしておく必要がある． - 制約が複数あり，ソルバーに渡すGが(M, N)の構造を持つ場合も考慮しないといけないので，以下の通りlistの長さをもとに場合分けして，様々な入力に対して適切なG_matを生成できるよう工夫している． + P: optimization weight matrix. shape=(N, N) + q: optimization weight vector. shape=(N,) or (N, 1) + G_list: constraint matrix list. [(N,), (N,), ...] + alpha_h_list: constraint value list Returns: - (str): status - (NDArray): optimal input. shape=(N,) + status: "optimal" on success + optimal_input: optimal input. shape=(N,) """ - assert isinstance(G_list, list) - assert isinstance(alpha_h_list, list) - - if len(G_list) > 1: - G = np.array(list(map(lambda x: x.flatten(), G_list))) - else: - G = G_list[0].reshape(1, -1) + G = _assemble_constraints(G_list) alpha_h = np.array(alpha_h_list) + x = _solve_qp(P, q, G, alpha_h) + return "optimal", x - try: - sol = self._set_qp_solvers(P, q, G, alpha_h) - return sol["status"], np.array(sol["x"]).flatten() - except Exception as e: - raise e +class CBFNomQPSolver: + """CBF-QP solver that tracks a nominal input.""" -class CBFNomQPSolver(QPSolver): def optimize( self, nominal_input: NDArray, P: NDArray, - G_list: List[NDArray], - alpha_h_list: List[float], - ) -> Tuple[str, NDArray]: - """ - Solve the following optimization problem + G_list: Sequence[NDArray], + alpha_h_list: list[float], + ) -> tuple[str, NDArray]: + """Solve the CBF-QP optimization problem with nominal input tracking. + minimize_{u} (1/2) * (u-nominal_input)^T*P*(u-nominal_input) subject to G*u + alpha(h) >= 0 Args: - nominal_input (NDArray): nominal_input. shape=(N,) - P (NDArray): optimization weight matrix. shape=(N, N) - G (List[NDArray]): constraint matrix list. [shape]=[(N,), (N,), ...] - alpha_h (List[float]): constraint value list + nominal_input: nominal input. shape=(N,) + P: optimization weight matrix. shape=(N, N) + G_list: constraint matrix list. [(N,), (N,), ...] + alpha_h_list: constraint value list Returns: - (str): status - (NDArray): optimal input. shape=(N,) + status: "optimal" on success + optimal_input: optimal input. shape=(N,) """ nominal_input = nominal_input.reshape(-1, 1) q = -P.T @ nominal_input - assert isinstance(G_list, list) - assert isinstance(alpha_h_list, list) - - if len(G_list) > 1: - G = -np.array(list(map(lambda x: x.flatten(), G_list))) - else: - G = -G_list[0].reshape(1, -1) + G = -_assemble_constraints(G_list) alpha_h = np.array(alpha_h_list) - - sol = self._set_qp_solvers(P, q, G, alpha_h) - return sol["status"], np.array(sol["x"]).flatten() + x = _solve_qp(P, q, G, alpha_h) + return "optimal", x diff --git a/docs/source/cbfpy.rst b/docs/source/cbfpy.rst new file mode 100644 index 0000000..e6271b2 --- /dev/null +++ b/docs/source/cbfpy.rst @@ -0,0 +1,16 @@ +API Reference +============= + +CBF Classes +----------- + +.. automodule:: cbfpy.cbf + :members: + :show-inheritance: + +QP Solvers +---------- + +.. automodule:: cbfpy.cbf_qp_solver + :members: + :show-inheritance: diff --git a/docs/source/examples.rst b/docs/source/examples.rst new file mode 100644 index 0000000..c6bbe47 --- /dev/null +++ b/docs/source/examples.rst @@ -0,0 +1,90 @@ +Examples +======== + +All examples can be run with: + +.. code-block:: bash + + python examples/example_{name}.py + +Each example creates a CBF optimizer wrapping a CBF and QP solver, +then runs a matplotlib animation showing the nominal input (black arrows) +vs. the CBF-filtered safe input (red arrows). + + +Scalar CBF +---------- + +Constrains a 1D state variable to stay above or below a limit. +The limit and direction change over time to demonstrate dynamic reconfiguration. + +.. literalinclude:: ../../examples/example_scalar_cbf.py + :language: python + :caption: examples/example_scalar_cbf.py + + +Scalar Range CBF +---------------- + +Constrains a 1D state variable to stay inside or outside a range ``[a, b]``. +Uses the barrier function :math:`h(x) = \left(\frac{b-a}{2}\right)^2 - \left(x - \frac{a+b}{2}\right)^2`. + +.. literalinclude:: ../../examples/example_scalar_range_cbf.py + :language: python + :caption: examples/example_scalar_range_cbf.py + + +Circle CBF +---------- + +Two agents move toward each other with a circular collision avoidance constraint. +Each agent treats the other's position as an obstacle with ``keep_inside=False``. + +.. literalinclude:: ../../examples/example_circle_cbf.py + :language: python + :caption: examples/example_circle_cbf.py + + +P-norm 2D CBF +------------- + +An agent avoids a p-norm (ellipsoidal) shaped area. +The p-norm shape generalizes circles (:math:`p=2`), diamonds (:math:`p=1`), and rectangles (:math:`p \to \infty`). + +.. literalinclude:: ../../examples/example_pnorm2d_cbf.py + :language: python + :caption: examples/example_pnorm2d_cbf.py + + +Unicycle Circle CBF +------------------- + +Circular area constraint for a unicycle model (input: linear velocity + angular velocity). +The unicycle kinematics are handled by an input transformation matrix. + +.. literalinclude:: ../../examples/example_unicycle_circle_cbf.py + :language: python + :caption: examples/example_unicycle_circle_cbf.py + + +Unicycle P-norm 2D CBF +---------------------- + +P-norm shaped area constraint for a unicycle model. +Combines the p-norm barrier function with unicycle input transformation. + +.. literalinclude:: ../../examples/example_unicycle_pnorm2d_cbf.py + :language: python + :caption: examples/example_unicycle_pnorm2d_cbf.py + + +LiDAR CBF +---------- + +LiDAR-based obstacle avoidance for a unicycle model. +Simulates a 2D LiDAR sensor and creates a CBF constraint for each ray, +enabling reactive navigation through environments with multiple obstacles. + +.. literalinclude:: ../../examples/example_lidar_cbf.py + :language: python + :caption: examples/example_lidar_cbf.py diff --git a/docs/source/index.rst b/docs/source/index.rst new file mode 100644 index 0000000..08aaaa9 --- /dev/null +++ b/docs/source/index.rst @@ -0,0 +1,50 @@ +cbfpy documentation +=================== + +**cbfpy** is a Python package for using simple Control Barrier Functions (CBF). + +CBF provides a framework for safety-constrained control by solving the optimization problem: + +.. math:: + + \min_{u} \quad & \frac{1}{2} (u - u_{\text{nom}})^T P (u - u_{\text{nom}}) \\ + \text{s.t.} \quad & \frac{\partial h}{\partial x} u + \alpha(h(x)) \geq 0 + +where :math:`h(x)` is the barrier function defining the safe set :math:`\{x \mid h(x) \geq 0\}`, +and :math:`\alpha` is an extended class-K function. + +Features +-------- + +- Symbolic CBF construction using SymPy (automatic gradient computation) +- Multiple CBF types: scalar, circular, p-norm, unicycle, LiDAR-based +- Lightweight QP solver via `quadprog `_ (Goldfarb/Idnani algorithm) +- Runtime parameter reconfiguration via ``set_parameters()`` + +Quick Start +----------- + +.. code-block:: python + + import numpy as np + from cbfpy import CircleCBF, CBFNomQPSolver + + # Create a circular CBF (keep agent inside a circle) + cbf = CircleCBF(center=np.zeros(2), radius=2.0, keep_inside=True) + + # Compute constraints at the current agent position + cbf.calc_constraints(agent_position=np.array([1.5, 0.0])) + G, alpha_h = cbf.get_constraints() + + # Solve the CBF-QP to get the safe input + solver = CBFNomQPSolver() + nominal_input = np.array([1.0, 0.0]) # desired direction + status, safe_input = solver.optimize(nominal_input, np.eye(2), [G], [alpha_h]) + + +.. toctree:: + :maxdepth: 4 + :caption: Contents: + + cbfpy + examples diff --git a/examples/example_circle_cbf.py b/examples/example_circle_cbf.py index b0acdf7..a881039 100755 --- a/examples/example_circle_cbf.py +++ b/examples/example_circle_cbf.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np @@ -13,50 +12,35 @@ class CBFOptimizer: - def __init__(self) -> None: + def __init__(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None: self.qp_nom_solver = CBFNomQPSolver() self.P = np.eye(2) - - self.circle_cbf = CircleCBF() - - # initialize(must be overwritten) - self.set_parameters(np.zeros(2)) + self.circle_cbf = CircleCBF(center, radius, keep_inside) def set_parameters(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None: self.circle_cbf.set_parameters(center, radius, keep_inside) - def get_parameters(self) -> Tuple[NDArray, float, bool]: + def get_parameters(self) -> tuple[NDArray, float, bool]: return self.circle_cbf.get_parameters() - def _calc_constraints(self, agent_position: NDArray) -> None: + def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> tuple[str, NDArray]: self.circle_cbf.calc_constraints(agent_position) - - def _get_constraints(self) -> Tuple[List[NDArray], List[float]]: G, alpha_h = self.circle_cbf.get_constraints() - return [G], [alpha_h] - - def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> Tuple[str, NDArray]: - self._calc_constraints(agent_position) - G_list, alpha_h_list = self._get_constraints() - - try: - return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h]) def main() -> None: - optimizer_list = [CBFOptimizer(), CBFOptimizer()] + optimizer_list = [CBFOptimizer(np.zeros(2)), CBFOptimizer(np.zeros(2))] initial_position_array = np.array([[-2, -2.5], [2, 2]]) - agent_position_list: List[NDArray] = [initial_position_array] + agent_position_list: list[NDArray] = [initial_position_array] dt = 0.1 fig, ax = plt.subplots() def update( frame: int, - agent_position_list: List[NDArray], + agent_position_list: list[NDArray], ) -> None: ax.cla() @@ -107,7 +91,7 @@ def update( ax.set_xlim(lim) ax.set_ylim(lim) - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=100, diff --git a/examples/example_lidar_cbf.py b/examples/example_lidar_cbf.py index 3b57593..7cbb689 100755 --- a/examples/example_lidar_cbf.py +++ b/examples/example_lidar_cbf.py @@ -1,8 +1,9 @@ #!/usr/bin/env python from abc import abstractmethod +from collections.abc import Sequence from dataclasses import dataclass -from typing import List, Sequence, Tuple, cast +from typing import cast import matplotlib.pyplot as plt import numpy as np @@ -10,57 +11,43 @@ from matplotlib.animation import FuncAnimation from numpy.typing import NDArray -from cbfpy.cbf import LiDARCBF +from cbfpy.cbf import LiDARCBF, rotation_matrix_2d from cbfpy.cbf_qp_solver import CBFNomQPSolver class CBFOptimizer: - def __init__(self, num_points: int) -> None: + def __init__(self, num_points: int, width: NDArray, keep_upper: bool = True) -> None: self.qp_nom_solver = CBFNomQPSolver() # Set weights so that angular velocity is more likely to occur. self.P = np.diag([10, 1]) - self.lidar_cbf_list = [LiDARCBF()] * num_points + self.lidar_cbf_list = [LiDARCBF(width, keep_upper) for _ in range(num_points)] def set_parameters(self, width: NDArray, keep_upper: bool = True) -> None: for lidar_cbf in self.lidar_cbf_list: lidar_cbf.set_parameters(width, keep_upper) - def get_parameters(self) -> List[Tuple[NDArray, bool]]: + def get_parameters(self) -> list[tuple[NDArray, bool]]: return [lidar_cbf.get_parameters() for lidar_cbf in self.lidar_cbf_list] - def calc_constraints(self, r: NDArray, theta: NDArray) -> None: + def optimize(self, nominal_input: NDArray, r: NDArray, theta: NDArray) -> tuple[str, NDArray]: + G_list: list[NDArray] = [] + alpha_h_list: list[float] = [] for i, lidar_cbf in enumerate(self.lidar_cbf_list): lidar_cbf.calc_constraints(r[i], theta[i]) - - def get_constraints(self) -> Tuple[List[NDArray], List[float]]: - G_list: List[NDArray] = [] - alpha_h_list: List[float] = [] - for lidar_cbf in self.lidar_cbf_list: G, alpha_h = lidar_cbf.get_constraints() G_list.append(G) alpha_h_list.append(alpha_h) - return G_list, alpha_h_list - - def optimize(self, nominal_input: NDArray, r: NDArray, theta: NDArray) -> Tuple[str, NDArray]: - self.calc_constraints(r, theta) - G_list, alpha_h_list = self.get_constraints() - - try: - return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list) class Obstacle: @abstractmethod - def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool: - ... + def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool: ... @abstractmethod - def plot_func(self, color: str, alpha: float) -> patches: - ... + def plot_func(self, color: str, alpha: float) -> patches.Patch: ... @dataclass @@ -84,7 +71,7 @@ def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool: <= self.radius, ) - def plot_func(self, color: str, alpha: float) -> patches: + def plot_func(self, color: str, alpha: float) -> patches.Patch: return patches.Circle(xy=self.center, radius=self.radius, color=color, alpha=alpha) @@ -95,11 +82,9 @@ class RectangleObstacle(Obstacle): theta: float def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool: - rotation_matrix = self._get_rotation_matrix(-self.theta) - # standardization to unit square transformed_position: NDArray = ( - rotation_matrix + rotation_matrix_2d(-self.theta) @ ( np.array( [r * np.cos(theta + curr_pose[2]), r * np.sin(theta + curr_pose[2])] + np.array(curr_pose[0:2]) @@ -112,7 +97,7 @@ def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool: # max norm return cast(bool, np.linalg.norm(transformed_position, ord=np.inf) <= 1) - def plot_func(self, color: str, alpha: float) -> patches: + def plot_func(self, color: str, alpha: float) -> patches.Patch: # matplotlib>=3.6 is required return patches.Rectangle( xy=self.center - self.width, @@ -124,15 +109,6 @@ def plot_func(self, color: str, alpha: float) -> patches: alpha=alpha, ) - @staticmethod - def _get_rotation_matrix(rad: float) -> NDArray: - return np.array( - [ - [np.cos(rad), -np.sin(rad)], - [np.sin(rad), np.cos(rad)], - ] - ) - class LiDARSimulator: def __init__( @@ -150,7 +126,7 @@ def __init__( self.range_step_num = range_step_num self.obstacle_list = obstacle_list - def sim(self, curr_pose: NDArray) -> Tuple[NDArray, NDArray]: + def sim(self, curr_pose: NDArray) -> tuple[NDArray, NDArray]: # in agent coordinate theta_array = np.array([2 * np.pi / self.num_points * i for i in range(self.num_points)]) range_step_array = np.array( @@ -166,17 +142,7 @@ def linear_search( curr_pose: NDArray, obstacle_list: Sequence[Obstacle], ) -> float: - """Linear search for measured distance - - Args: - range_step_array (NDArray): candidate sequence of measured distances - theta (float): laser angle in agent coordinate - curr_pose (NDArray): current pose(x,y,theta) - obstacle_list (Sequence[Obstacle]): obstacles - - Returns: - float: measured distance - """ + """Linear search for measured distance.""" for range_ in range_step_array: if any([obstacle.is_inner(range_, theta, curr_pose) for obstacle in obstacle_list]): return float(range_) @@ -195,11 +161,11 @@ def linear_search( def main() -> None: num_points = 20 - optimizer = CBFOptimizer(num_points) - optimizer.set_parameters(np.array([0.7, 0.5])) + width = np.array([0.7, 0.5]) + optimizer = CBFOptimizer(num_points, width) initial_pose = np.array([0, -3, -0.1]) - agent_pose_list: List[NDArray] = [initial_pose] + agent_pose_list: list[NDArray] = [initial_pose] dt = 0.1 # set obstacles @@ -216,7 +182,7 @@ def main() -> None: fig, ax = plt.subplots() - def update(frame: int, agent_pose_list: List[NDArray]) -> None: + def update(frame: int, agent_pose_list: list[NDArray]) -> None: ax.cla() curr_pose = agent_pose_list[-1] @@ -302,16 +268,16 @@ def update(frame: int, agent_pose_list: List[NDArray]) -> None: ax.plot([0], [0], linewidth=5, color="red", label="optimal_input") param_list = optimizer.get_parameters() - width = param_list[0][0] - r = patches.Ellipse( + cbf_width = param_list[0][0] + r_patch = patches.Ellipse( xy=curr_position, - width=width[0] * 2, - height=width[1] * 2, + width=cbf_width[0] * 2, + height=cbf_width[1] * 2, angle=curr_theta * 180 / np.pi, color="blue", alpha=0.5, ) - ax.add_patch(r) + ax.add_patch(r_patch) for obstacle in obstacle_list: ax.add_patch(obstacle.plot_func(color="green", alpha=0.5)) @@ -323,7 +289,7 @@ def update(frame: int, agent_pose_list: List[NDArray]) -> None: ax.set_ylim(lim) ax.legend(loc="upper left") - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=500, diff --git a/examples/example_pnorm2d_cbf.py b/examples/example_pnorm2d_cbf.py index 2bb201f..45f627e 100755 --- a/examples/example_pnorm2d_cbf.py +++ b/examples/example_pnorm2d_cbf.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np @@ -13,47 +12,28 @@ class CBFOptimizer: - def __init__(self) -> None: + def __init__( + self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True + ) -> None: self.qp_nom_solver = CBFNomQPSolver() self.P = np.eye(2) - - self.pnorm2d_cbf = Pnorm2dCBF() - - # initialize(must be overwritten) - self.set_parameters(np.zeros(2), np.ones(2)) + self.pnorm2d_cbf = Pnorm2dCBF(center, width, theta, p, keep_inside) def set_parameters( self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True ) -> None: self.pnorm2d_cbf.set_parameters(center, width, theta, p, keep_inside) - def get_parameters(self) -> Tuple[NDArray, NDArray, float, float, bool]: + def get_parameters(self) -> tuple[NDArray, NDArray, float, float, bool]: return self.pnorm2d_cbf.get_parameters() - def _calc_constraints(self, agent_position: NDArray) -> None: + def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> tuple[str, NDArray]: self.pnorm2d_cbf.calc_constraints(agent_position) - - def _get_constraints(self) -> Tuple[List[NDArray], List[float]]: G, alpha_h = self.pnorm2d_cbf.get_constraints() - return [G], [alpha_h] - - def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> Tuple[str, NDArray]: - self._calc_constraints(agent_position) - G_list, alpha_h_list = self._get_constraints() - - try: - return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h]) def main() -> None: - optimizer = CBFOptimizer() - - initial_position = np.array([-3, -2.5]) - agent_position_list: List[NDArray] = [initial_position] - dt = 0.1 - # obstacle center = np.array([-0.5, 0.5]) width = np.array([3, 2]) @@ -61,14 +41,18 @@ def main() -> None: p = 2.0 keep_inside = False - optimizer.set_parameters(center, width, theta, p, keep_inside) + optimizer = CBFOptimizer(center, width, theta, p, keep_inside) + + initial_position = np.array([-3, -2.5]) + agent_position_list: list[NDArray] = [initial_position] + dt = 0.1 nominal_input = np.ones(2) fig, ax = plt.subplots() def update( frame: int, - agent_position_list: List[NDArray], + agent_position_list: list[NDArray], ) -> None: ax.cla() @@ -107,7 +91,7 @@ def update( ax.set_xlim(lim) ax.set_ylim(lim) - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=100, diff --git a/examples/example_scalar_cbf.py b/examples/example_scalar_cbf.py index e77a32f..96adfd0 100755 --- a/examples/example_scalar_cbf.py +++ b/examples/example_scalar_cbf.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np @@ -12,52 +11,37 @@ class CBFOptimizer: - def __init__(self) -> None: + def __init__(self, limit: float, keep_upper: bool = True) -> None: self.qp_nom_solver = CBFNomQPSolver() self.P = np.eye(1) - - self.scalar_cbf = ScalarCBF() - - # initialize (must be overwritten) - self.set_parameters(0.0) + self.scalar_cbf = ScalarCBF(limit, keep_upper) def set_parameters(self, limit: float, keep_upper: bool = True) -> None: self.scalar_cbf.set_parameters(limit, keep_upper) - def get_parameters(self) -> Tuple[float, bool]: + def get_parameters(self) -> tuple[float, bool]: return self.scalar_cbf.get_parameters() - def _calc_constraints(self, curr_value: float) -> None: + def optimize(self, nominal_input: float, curr_value: float) -> tuple[str, NDArray]: self.scalar_cbf.calc_constraints(curr_value) - - def _get_constraints(self) -> Tuple[List[NDArray], List[float]]: G, alpha_h = self.scalar_cbf.get_constraints() - return [G], [alpha_h] - - def optimize(self, nominal_input: float, curr_value: float) -> Tuple[str, NDArray]: - self._calc_constraints(curr_value) - G_list, alpha_h_list = self._get_constraints() - - try: - return self.qp_nom_solver.optimize(np.array(nominal_input), self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(np.array(nominal_input), self.P, [G], [alpha_h]) def main() -> None: - optimizer = CBFOptimizer() + optimizer = CBFOptimizer(limit=0.0) initial_value = 0.0 - value_list: List[float] = [initial_value] - time_list: List[float] = [0.0] + value_list: list[float] = [initial_value] + time_list: list[float] = [0.0] dt = 0.1 fig, ax = plt.subplots() def update( frame: int, - value_list: List[float], - time_list: List[float], + value_list: list[float], + time_list: list[float], ) -> None: ax.cla() @@ -109,7 +93,7 @@ def update( ax.set_ylim([-3, 3]) ax.legend(loc="upper right") - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=100, diff --git a/examples/example_scalar_range_cbf.py b/examples/example_scalar_range_cbf.py index 7acae57..7622d0f 100755 --- a/examples/example_scalar_range_cbf.py +++ b/examples/example_scalar_range_cbf.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np @@ -12,52 +11,37 @@ class CBFOptimizer: - def __init__(self) -> None: + def __init__(self, a: float, b: float, keep_inside: bool = True) -> None: self.qp_nom_solver = CBFNomQPSolver() self.P = np.eye(1) - - self.scalar_range_cbf = ScalarRangeCBF() - - # initialize (must be overwritten) - self.set_parameters(0.0, 1.0) + self.scalar_range_cbf = ScalarRangeCBF(a, b, keep_inside) def set_parameters(self, a: float, b: float, keep_inside: bool = True) -> None: self.scalar_range_cbf.set_parameters(a, b, keep_inside) - def get_parameters(self) -> Tuple[float, float, bool]: + def get_parameters(self) -> tuple[float, float, bool]: return self.scalar_range_cbf.get_parameters() - def _calc_constraints(self, curr_value: float) -> None: + def optimize(self, nominal_input: float, curr_value: float) -> tuple[str, NDArray]: self.scalar_range_cbf.calc_constraints(curr_value) - - def _get_constraints(self) -> Tuple[List[NDArray], List[float]]: G, alpha_h = self.scalar_range_cbf.get_constraints() - return [G], [alpha_h] - - def optimize(self, nominal_input: float, curr_value: float) -> Tuple[str, NDArray]: - self._calc_constraints(curr_value) - G_list, alpha_h_list = self._get_constraints() - - try: - return self.qp_nom_solver.optimize(np.array(nominal_input), self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(np.array(nominal_input), self.P, [G], [alpha_h]) def main() -> None: - optimizer = CBFOptimizer() + optimizer = CBFOptimizer(a=0.0, b=1.0) initial_value = 0.0 - value_list: List[float] = [initial_value] - time_list: List[float] = [0.0] + value_list: list[float] = [initial_value] + time_list: list[float] = [0.0] dt = 0.1 fig, ax = plt.subplots() def update( frame: int, - value_list: List[float], - time_list: List[float], + value_list: list[float], + time_list: list[float], ) -> None: ax.cla() @@ -114,7 +98,7 @@ def update( ax.set_ylim([-3, 3]) ax.legend(loc="upper right") - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=100, diff --git a/examples/example_unicycle_circle_cbf.py b/examples/example_unicycle_circle_cbf.py index 2160575..bf1d84a 100755 --- a/examples/example_unicycle_circle_cbf.py +++ b/examples/example_unicycle_circle_cbf.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np @@ -13,55 +12,39 @@ class CBFOptimizer: - def __init__(self) -> None: + def __init__(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None: self.qp_nom_solver = CBFNomQPSolver() self.P = np.eye(2) - - self.circle_cbf = UnicycleCircleCBF() - - # initialize(must be overwritten) - self.set_parameters(np.zeros(2)) + self.circle_cbf = UnicycleCircleCBF(center, radius, keep_inside) def set_parameters(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None: self.circle_cbf.set_parameters(center, radius, keep_inside) - def get_parameters(self) -> Tuple[NDArray, float, bool]: + def get_parameters(self) -> tuple[NDArray, float, bool]: return self.circle_cbf.get_parameters() - def _calc_constraints(self, agent_pose: NDArray) -> None: + def optimize(self, nominal_input: NDArray, agent_pose: NDArray) -> tuple[str, NDArray]: self.circle_cbf.calc_constraints(agent_pose) - - def _get_constraints(self) -> Tuple[List[NDArray], List[float]]: G, alpha_h = self.circle_cbf.get_constraints() - return [G], [alpha_h] - - def optimize(self, nominal_input: NDArray, agent_pose: NDArray) -> Tuple[str, NDArray]: - self._calc_constraints(agent_pose) - G_list, alpha_h_list = self._get_constraints() - - try: - return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h]) def main() -> None: - optimizer = CBFOptimizer() - - initial_pose_array = np.array([0, 0.5, 0.0]) - agent_pose_list: List[NDArray] = [initial_pose_array] - dt = 0.1 center = np.array([-0.5, 0.5]) radius = 1.5 keep_inside = True - optimizer.set_parameters(center, radius, keep_inside) + optimizer = CBFOptimizer(center, radius, keep_inside) + + initial_pose_array = np.array([0, 0.5, 0.0]) + agent_pose_list: list[NDArray] = [initial_pose_array] + dt = 0.1 fig, ax = plt.subplots() def update( frame: int, - agent_pose_list: List[NDArray], + agent_pose_list: list[NDArray], ) -> None: ax.cla() @@ -145,7 +128,7 @@ def update( ax.set_xlim(lim) ax.set_ylim(lim) - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=100, diff --git a/examples/example_unicycle_pnorm2d_cbf.py b/examples/example_unicycle_pnorm2d_cbf.py index b6fd1ca..fa02fcd 100755 --- a/examples/example_unicycle_pnorm2d_cbf.py +++ b/examples/example_unicycle_pnorm2d_cbf.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -from typing import List, Tuple import matplotlib.pyplot as plt import numpy as np @@ -13,59 +12,45 @@ class CBFOptimizer: - def __init__(self) -> None: + def __init__( + self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True + ) -> None: self.qp_nom_solver = CBFNomQPSolver() self.P = np.diag([1, 10]) - - self.pnorm2d_cbf = UnicyclePnorm2dCBF() - - # initialize(must be overwritten) - self.set_parameters(np.zeros(2), np.ones(2)) + self.pnorm2d_cbf = UnicyclePnorm2dCBF(center, width, theta, p, keep_inside) def set_parameters( self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True ) -> None: self.pnorm2d_cbf.set_parameters(center, width, theta, p, keep_inside) - def get_parameters(self) -> Tuple[NDArray, NDArray, float, float, bool]: + def get_parameters(self) -> tuple[NDArray, NDArray, float, float, bool]: return self.pnorm2d_cbf.get_parameters() - def _calc_constraints(self, agent_pose: NDArray) -> None: + def optimize(self, nominal_input: NDArray, agent_pose: NDArray) -> tuple[str, NDArray]: self.pnorm2d_cbf.calc_constraints(agent_pose) - - def _get_constraints(self) -> Tuple[List[NDArray], List[float]]: G, alpha_h = self.pnorm2d_cbf.get_constraints() - return [G], [alpha_h] - - def optimize(self, nominal_input: NDArray, agent_pose: NDArray) -> Tuple[str, NDArray]: - self._calc_constraints(agent_pose) - G_list, alpha_h_list = self._get_constraints() - - try: - return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list) - except Exception as e: - raise e + return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h]) def main() -> None: - optimizer = CBFOptimizer() - - initial_pose_array = np.array([-1, -1, 0]) - agent_pose_list: List[NDArray] = [initial_pose_array] - dt = 0.1 center = np.array([-0.5, 0.5]) width = np.array([3, 2]) theta = -0.3 p = 2.0 keep_inside = True - optimizer.set_parameters(center, width, theta, p, keep_inside) + optimizer = CBFOptimizer(center, width, theta, p, keep_inside) + + initial_pose_array = np.array([-1, -1, 0]) + agent_pose_list: list[NDArray] = [initial_pose_array] + dt = 0.1 fig, ax = plt.subplots() def update( frame: int, - agent_pose_list: List[NDArray], + agent_pose_list: list[NDArray], ) -> None: ax.cla() @@ -151,7 +136,7 @@ def update( ax.set_xlim(lim) ax.set_ylim(lim) - ani = FuncAnimation( + ani = FuncAnimation( # noqa: F841 fig, update, frames=200, diff --git a/pyproject.toml b/pyproject.toml index d88f195..72fd475 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,108 +1,72 @@ -[tool.poetry] +[project] name = "cbfpy" version = "0.1.0" description = "Python package for using simple control barrier function" -authors = ["Toshiyuki Oshima "] -license = "Apache License 2.0" readme = "README.md" - -packages = [ - { include = "cbfpy" }, - { include = "examples" }, - { include = "tests" }, +license = "Apache-2.0" +requires-python = ">=3.10" +authors = [{ name = "Toshiyuki Oshima", email = "toshiyuki67026@gmail.com" }] +dependencies = [ + "sympy>=1.12", + "quadprog>=0.1.12", + "matplotlib>=3.7", + "numpy>=1.24", ] -# duplicate definition with tool.poetry for sphinx -[project] -name = "cbfpy" -version = "0.1.0" -description = "Python package for using simple control barrier function" - -[[project.authors]] -name = "Toshiyuki Oshima" - -[tool.poetry.dependencies] -python = "^3.8.10" -sympy = "^1.11.1" -cvxopt = "^1.3.0" -matplotlib = "^3.6.2" - -[tool.poetry.group.dev.dependencies] -black = "^22.12.0" -isort = "^5.11.4" -mypy = "^0.991" -sphinx = "5.3.0" -sphinx-rtd-theme = "^1.1.1" -sphinx-pyproject = "^0.1.0" -taskipy = "^1.10.3" -pre-commit = "^2.21.0" -ruff = "^0.0.213" - -[tool.poetry.group.test.dependencies] -pytest = "^7.2.0" -pytest-cov = "^4.0.0" -coverage = "^7.0.3" - [build-system] -requires = ["poetry-core>=1.0.0"] -build-backend = "poetry.core.masonry.api" +requires = ["hatchling"] +build-backend = "hatchling.build" -[tool.black] -line-length = 119 -target-version = ['py38'] -include = '\.pyi?$' -# automatically ignore files in .gitignore -extend-exclude = ''' -( - \.git - | \.vscode - | assets -) -''' +[tool.hatch.build.targets.wheel] +packages = ["cbfpy"] -[tool.isort] -profile = "black" -src_paths = ["cbfpy", "examples", "tests"] -line_length = 119 -# ignore files in .gitignore -skip_gitignore = true +[project.optional-dependencies] +dev = [ + "ruff>=0.9", + "mypy>=1.14", + "sphinx>=7.4", + "sphinx-rtd-theme>=3.0", + "sphinx-pyproject>=0.3", + "pre-commit>=4.0", +] +test = [ + "pytest>=8.0", + "pytest-cov>=6.0", +] [tool.ruff] line-length = 119 -select = ["E", "F"] -ignore = ["E402", "F841"] -# automatically ignore files in .gitignore -exclude = [".git", ".vscode", "assets"] +target-version = "py310" + +[tool.ruff.lint] +select = ["E", "F", "I", "UP"] +ignore = ["E402"] + +[tool.ruff.lint.isort] +known-first-party = ["cbfpy"] [tool.mypy] -python_version = 3.8 +python_version = "3.10" plugins = "numpy.typing.mypy_plugin" strict = true ignore_missing_imports = true disallow_any_generics = false +exclude = ["examples/"] [tool.pytest.ini_options] testpaths = ["tests"] -addopts = "--cov=cbfpy --cov-branch --cov-report xml --cov-report html:cov_html" +addopts = "--cov=cbfpy --cov-branch --cov-report=xml --cov-report=html:cov_html" [tool.sphinx-pyproject] project = "cbfpy" -copyright = "2022, Toshiyuki Oshima" +copyright = "2026, Toshiyuki Oshima" language = "en" package_root = "cbfpy" html_theme = "sphinx_rtd_theme" todo_include_todos = true -templates_path = ["_templates"] -html_static_path = ["_static"] extensions = [ "sphinx.ext.autodoc", "sphinx.ext.viewcode", "sphinx.ext.todo", "sphinx.ext.napoleon", ] - -[tool.taskipy.tasks] -test = "pytest" -fmt = "isort . && black ." -lint = "black --check . && ruff . && mypy ." -docs = "sphinx-apidoc -F -o docs/source cbfpy && sphinx-build docs/source docs/build" diff --git a/tests/test_cbf.py b/tests/test_cbf.py index 96bd79d..67b7fa4 100755 --- a/tests/test_cbf.py +++ b/tests/test_cbf.py @@ -17,26 +17,26 @@ class TestCBFBase: - G = np.ones(2) - h = 1.0 - cbf_base = CBFBase(G=G, h=h) - def test_get_constraints(self) -> None: - G, alpha_h = self.cbf_base.get_constraints() - assert np.allclose(G, self.G) - assert np.allclose(alpha_h, self.cbf_base._alpha(self.h)) + G = np.ones(2) + h = 1.0 + cbf_base = CBFBase() + cbf_base.G = G + cbf_base.h = h + ret_G, alpha_h = cbf_base.get_constraints() + assert np.allclose(ret_G, G) + assert np.allclose(alpha_h, cbf_base._alpha(h)) class TestGeneralCBF: - general_cbf = GeneralCBF(G=np.ones(2), h=1.0) - def test_calc_constraints(self) -> None: + general_cbf = GeneralCBF(G=np.ones(2), h=1.0) G = 2 * np.ones(2) h = 2.0 - self.general_cbf.calc_constraints(G, h) - ret_G, ret_alpha_h = self.general_cbf.get_constraints() + general_cbf.calc_constraints(G, h) + ret_G, ret_alpha_h = general_cbf.get_constraints() assert np.allclose(ret_G, G) - assert np.allclose(ret_alpha_h, self.general_cbf._alpha(h)) + assert np.allclose(ret_alpha_h, general_cbf._alpha(h)) class TestScalarCBF: @@ -44,15 +44,13 @@ class TestScalarCBF: keep_upper = True def test_set_and_get_parameters(self) -> None: - scalar_cbf = ScalarCBF() - scalar_cbf.set_parameters(self.limit, self.keep_upper) + scalar_cbf = ScalarCBF(self.limit, self.keep_upper) limit, keep_upper = scalar_cbf.get_parameters() assert limit == pytest.approx(self.limit) assert keep_upper == self.keep_upper def test_calc_constraints(self) -> None: - scalar_cbf = ScalarCBF() - scalar_cbf.set_parameters(self.limit, self.keep_upper) + scalar_cbf = ScalarCBF(self.limit, self.keep_upper) scalar_cbf.calc_constraints(3.0) G, alpha_h = scalar_cbf.get_constraints() assert np.allclose(G, np.array(1.0)) @@ -65,16 +63,14 @@ class TestScalarRangeCBF: keep_inside = True def test_set_and_get_parameters(self) -> None: - scalar_range_cbf = ScalarRangeCBF() - scalar_range_cbf.set_parameters(self.a, self.b, self.keep_inside) + scalar_range_cbf = ScalarRangeCBF(self.a, self.b, self.keep_inside) a, b, keep_inside = scalar_range_cbf.get_parameters() assert a == pytest.approx(self.a) assert b == pytest.approx(self.b) assert keep_inside == self.keep_inside def test_calc_constraints(self) -> None: - scalar_range_cbf = ScalarRangeCBF() - scalar_range_cbf.set_parameters(self.a, self.b, self.keep_inside) + scalar_range_cbf = ScalarRangeCBF(self.a, self.b, self.keep_inside) scalar_range_cbf.calc_constraints(2.0) G, alpha_h = scalar_range_cbf.get_constraints() @@ -88,16 +84,14 @@ class TestCircleCBF: keep_inside = True def test_set_and_get_parameters(self) -> None: - circle_cbf = CircleCBF() - circle_cbf.set_parameters(self.center, self.radius, self.keep_inside) + circle_cbf = CircleCBF(self.center, self.radius, self.keep_inside) center, radius, keep_inside = circle_cbf.get_parameters() assert np.allclose(center, self.center) assert radius == pytest.approx(self.radius) assert keep_inside == self.keep_inside def test_calc_constraints(self) -> None: - circle_cbf = CircleCBF() - circle_cbf.set_parameters(self.center, self.radius, self.keep_inside) + circle_cbf = CircleCBF(self.center, self.radius, self.keep_inside) agent_position = 2 * np.ones(2) circle_cbf.calc_constraints(agent_position) @@ -112,16 +106,14 @@ class TestUnicycleCircleCBF: keep_inside = True def test_set_and_get_parameters(self) -> None: - circle_cbf = UnicycleCircleCBF() - circle_cbf.set_parameters(self.center, self.radius, self.keep_inside) + circle_cbf = UnicycleCircleCBF(self.center, self.radius, self.keep_inside) center, radius, keep_inside = circle_cbf.get_parameters() assert np.allclose(center, self.center) assert radius == pytest.approx(self.radius) assert keep_inside == self.keep_inside def test_calc_constraints(self) -> None: - circle_cbf = UnicycleCircleCBF() - circle_cbf.set_parameters(self.center, self.radius, self.keep_inside) + circle_cbf = UnicycleCircleCBF(self.center, self.radius, self.keep_inside) agent_pose = 2 * np.ones(3) circle_cbf.calc_constraints(agent_pose) @@ -138,8 +130,7 @@ class TestPnorm2dCBF: keep_inside = True def test_set_and_get_parameters(self) -> None: - pnorm2d_cbf = Pnorm2dCBF() - pnorm2d_cbf.set_parameters(self.center, self.width, self.theta, self.p, self.keep_inside) + pnorm2d_cbf = Pnorm2dCBF(self.center, self.width, self.theta, self.p, self.keep_inside) center, width, theta, p, keep_inside = pnorm2d_cbf.get_parameters() assert np.allclose(center, self.center) assert np.allclose(width, self.width) @@ -148,8 +139,7 @@ def test_set_and_get_parameters(self) -> None: assert keep_inside == self.keep_inside def test_calc_constraints(self) -> None: - pnorm2d_cbf = Pnorm2dCBF() - pnorm2d_cbf.set_parameters(self.center, self.width, self.theta, self.p, self.keep_inside) + pnorm2d_cbf = Pnorm2dCBF(self.center, self.width, self.theta, self.p, self.keep_inside) agent_position = 2 * np.ones(2) pnorm2d_cbf.calc_constraints(agent_position) @@ -166,8 +156,7 @@ class TestUnicyclePnorm2dCBF: keep_inside = True def test_set_and_get_parameters(self) -> None: - pnorm2d_cbf = UnicyclePnorm2dCBF() - pnorm2d_cbf.set_parameters(self.center, self.width, self.theta, self.p, self.keep_inside) + pnorm2d_cbf = UnicyclePnorm2dCBF(self.center, self.width, self.theta, self.p, self.keep_inside) center, width, theta, p, keep_inside = pnorm2d_cbf.get_parameters() assert np.allclose(center, self.center) assert np.allclose(width, self.width) @@ -176,8 +165,7 @@ def test_set_and_get_parameters(self) -> None: assert keep_inside == self.keep_inside def test_calc_constraints(self) -> None: - pnorm2d_cbf = UnicyclePnorm2dCBF() - pnorm2d_cbf.set_parameters(self.center, self.width, self.theta, self.p, self.keep_inside) + pnorm2d_cbf = UnicyclePnorm2dCBF(self.center, self.width, self.theta, self.p, self.keep_inside) agent_pose = 2 * np.ones(3) pnorm2d_cbf.calc_constraints(agent_pose) @@ -191,15 +179,13 @@ class TestLiDARCBF: keep_upper = True def test_set_and_get_parameters(self) -> None: - lidar_cbf = LiDARCBF() - lidar_cbf.set_parameters(self.width, self.keep_upper) + lidar_cbf = LiDARCBF(self.width, self.keep_upper) width, keep_upper = lidar_cbf.get_parameters() assert np.allclose(width, self.width) assert keep_upper == self.keep_upper def test_calc_constraints(self) -> None: - lidar_cbf = LiDARCBF() - lidar_cbf.set_parameters(self.width, self.keep_upper) + lidar_cbf = LiDARCBF(self.width, self.keep_upper) lidar_cbf.calc_constraints(3.0, 0.0) G, alpha_h = lidar_cbf.get_constraints() diff --git a/tests/test_cbf_qp_solver.py b/tests/test_cbf_qp_solver.py index 9fcba3a..8db35af 100755 --- a/tests/test_cbf_qp_solver.py +++ b/tests/test_cbf_qp_solver.py @@ -12,32 +12,33 @@ class TestCBFQPSolver: q = np.ones(2) def test_optimize_optimal1(self) -> None: - """one inequality constraint""" + """one inequality constraint (inactive)""" G_list = [np.ones(2)] alpha_h_list = [1.0] status, optimal_input = self.qp_solver.optimize(self.P, self.q, G_list, alpha_h_list) assert status == "optimal" - assert np.allclose(optimal_input, np.array([-1.00188955, -1.00188955])) + # Unconstrained minimum: x = -q = [-1, -1], constraint [1,1]x<=1 is inactive + assert np.allclose(optimal_input, np.array([-1.0, -1.0])) def test_optimize_optimal2(self) -> None: - """two inequality constraints""" + """two inequality constraints (one active)""" G_list = [np.ones(2), -np.ones(2)] alpha_h_list = [1.0, 1.0] status, optimal_input = self.qp_solver.optimize(self.P, self.q, G_list, alpha_h_list) assert status == "optimal" - assert np.allclose(optimal_input, np.array([-0.2, -0.2])) + # Active constraint: x1+x2 = -1, by symmetry x1=x2=-0.5 + assert np.allclose(optimal_input, np.array([-0.5, -0.5])) def test_optimize_except(self) -> None: - """exception""" + """dimension mismatch raises exception""" G_list = [np.ones(2), -np.ones(2)] alpha_h_list = [1.0] assert len(G_list) != len(alpha_h_list) - with pytest.raises(Exception) as e: + with pytest.raises(Exception): _ = self.qp_solver.optimize(self.P, self.q, G_list, alpha_h_list) - assert str(e.value) == "'G' must be a 'd' matrix of size (1, 2)" class TestCBFNomQPSolver: @@ -46,31 +47,32 @@ class TestCBFNomQPSolver: nominal_input = np.ones(2) def test_optimize_optimal1(self) -> None: - """one inequality constraint""" + """one inequality constraint (inactive)""" G_list = [np.ones(2)] alpha_h_list = [1.0] status, optimal_input = self.nom_qp_solver.optimize(self.nominal_input, self.P, G_list, alpha_h_list) assert status == "optimal" - assert np.allclose(optimal_input, np.array([1.00188955, 1.00188955])) + # Unconstrained minimum: u = nominal_input = [1, 1], constraint is inactive + assert np.allclose(optimal_input, np.array([1.0, 1.0])) def test_optimize_optimal2(self) -> None: - """two inequality constraints""" + """two inequality constraints (one active)""" G_list = [np.ones(2), -np.ones(2)] alpha_h_list = [1.0, 1.0] status, optimal_input = self.nom_qp_solver.optimize(self.nominal_input, self.P, G_list, alpha_h_list) assert status == "optimal" - assert np.allclose(optimal_input, np.array([0.2, 0.2])) + # Active constraint: u1+u2 = 1, by symmetry u1=u2=0.5 + assert np.allclose(optimal_input, np.array([0.5, 0.5])) def test_optimize_except(self) -> None: - """exception""" + """dimension mismatch raises exception""" G_list = [np.ones(2), -np.ones(2)] alpha_h_list = [1.0] assert len(G_list) != len(alpha_h_list) - with pytest.raises(Exception) as e: + with pytest.raises(Exception): _ = self.nom_qp_solver.optimize(self.nominal_input, self.P, G_list, alpha_h_list) - assert str(e.value) == "'G' must be a 'd' matrix of size (1, 2)"