Adding CBF and bump a new version (#19)

* Adding CBF and bump a new version

* Fix a minor bug

* wip

Project.toml

@@ -1,17 +1,21 @@
 name = "FSimZoo"
 uuid = "e2d5807d-1594-43f7-91a6-54c6eef5a9d2"
 authors = ["JinraeKim <[email protected]> and contributors"]
-version = "0.5.1"
+version = "0.6.0"
 
 [deps]
 ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66"
+Convex = "f65535da-76fb-5f13-bab9-19810c17039a"
+ECOS = "e2685f51-7e38-5353-a97d-a921fd2c8199"
 FSimBase = "731e945d-f3ed-49a1-bee2-3998705ef155"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 MatrixEquations = "99c1a7ee-ab34-5fd5-8076-27c950a045f4"
 
 [compat]
 ComponentArrays = "0.13"
+Convex = "0.15"
+ECOS = "1.1"
 FSimBase = "0.2"
 ForwardDiff = "0.10"
 MatrixEquations = "2.1"

README.md

@@ -40,11 +40,13 @@ contains predefined environments and controllers for [FlightSims.jl](https://git
     - (Linear quadratic regulator) `LQR`
     - (Proportional-Integral-Derivative controller) `PID`
         - Note that the derivative term is obtained via second-order filter.
+    - (Pure proportional navigation guidance) `PPNG`
     - (For multicopter position tracking)
         - `BacksteppingPositionController` (control input: `T_dot`, `M`)
         - `GeometricTrackingController` (control input: `T`, `M`)
         - `InnerLoopGeometricTrackingController` and `OuterLoopGeometricTrackingController` (based on `GeometricTrackingController` but seperated for a hierarchical structure)
-    - (Pure proportional navigation guidance) `PPNG`
+    - (Safety filters via control barrier functions (CBFs))
+        - (Position CBF for input-affine systems) `InputAffinePositionCBF`
 
     </details>
 

src/FSimZoo.jl

@@ -8,6 +8,7 @@ using ComponentArrays
 using ForwardDiff
 using MatrixEquations
 using LinearAlgebra
+using Convex, ECOS
 
 ## basics
 export TwoDimensionalNonlinearPolynomialSystem, LinearSystem, ReferenceModel, MultipleEnvs
@@ -16,6 +17,7 @@ export TwoDimensionalNonlinearDTSystem
 ## controllers
 export LQR, PID, BacksteppingPositionController
 export GeometricTrackingController, OuterLoopGeometricTrackingController, InnerLoopGeometricTrackingController
+export InputAffinePositionCBF
 ## multicopters
 export Multicopter
 export Quadcopter, IslamQuadcopter, GoodarziQuadcopter, LeeQuadcopter

src/environments/controllers/CBF.jl

@@ -0,0 +1,40 @@
+struct InputAffinePositionCBF <: AbstractEnv
+    f
+    g
+    h
+    α1
+    α2
+end
+
+
+"""
+p in R^3
+v in R^3
+u: Convex.Variable(m)
+"""
+function generate_constraint(cbf::InputAffinePositionCBF, p, v, u)
+    (; f, g, h, α1, α2) = cbf
+    ∇h = p -> ForwardDiff.gradient(h, p)
+    h1 = (p, v) -> ∇h(p)' * v + α1(h(p))
+    ∇h1_p = (p, v) -> ForwardDiff.gradient(p -> h1(p, v), p)
+    ∇h1_v = (p, v) -> ForwardDiff.gradient(v -> h1(p, v), v)
+    h2 = (p, v) -> ∇h1_p(p, v)' * v + ∇h1_v(p, v)' * (f(p, v)+g(p, v)*u) + α2(h1(p, v))
+    constraint = h2(p, v) >= 0.0
+end
+
+
+"""
+p in R^3
+v in R^3
+u_nom: nominal control input, in R^m
+"""
+function Command(
+        cbf::InputAffinePositionCBF, u, p, v, u_nom, constraints;
+        solver=ECOS.Optimizer, silent_solver=true,
+    )
+    cbf_constraint = generate_constraint(cbf, p, v, u)
+    constraints = [constraints..., cbf_constraint]
+    problem = minimize(sumsquares(u - u_nom), constraints)
+    solve!(problem, solver; silent_solver=silent_solver)
+    return reshape(u.value, size(u_nom)...)
+end

src/environments/controllers/controllers.jl

@@ -2,3 +2,4 @@ include("BacksteppingPositionController.jl")
 include("GeometricTrackingController.jl")
 include("LQR.jl")
 include("PID.jl")
+include("CBF.jl")

test/cbf.jl

@@ -0,0 +1,26 @@
+using Test
+using FSimZoo
+using FSimBase
+using LinearAlgebra
+using Convex
+
+
+function test_InputAffinePositionCBF()
+    f = (p, v) -> zeros(3) + [0, 0, 9.81]
+    g = (p, v) -> I(3)
+    h = p -> -p[3] - (-0.1)  # >= 0
+    α1 = x -> 5*x
+    α2 = x -> 5*x
+    cbf = InputAffinePositionCBF(f, g, h, α1, α2)
+    # filter
+    p = zeros(3)
+    v = zeros(3)
+    u_nom = zeros(3)
+    u = Convex.Variable(length(u_nom))
+    u = Command(cbf, u, p, v, u_nom, [])
+end
+
+
+@testset "cbf" begin
+    test_InputAffinePositionCBF()
+end

test/runtests.jl

@@ -4,4 +4,5 @@ using Test
 @testset "FSimZoo.jl" begin
     include("geometric_tracking.jl")
     include("geometric_tracking_inner_outer.jl")
+    include("cbf.jl")
 end