From 9c33e0049f808bf21491ec41ee03e89f2092f9d1 Mon Sep 17 00:00:00 2001 From: Jinrae Kim Date: Tue, 2 May 2023 16:36:09 +0900 Subject: [PATCH] Adding CBF and bump a new version (#19) * Adding CBF and bump a new version * Fix a minor bug * wip --- Project.toml | 6 +++- README.md | 4 ++- src/FSimZoo.jl | 2 ++ src/environments/controllers/CBF.jl | 40 +++++++++++++++++++++ src/environments/controllers/controllers.jl | 1 + test/cbf.jl | 26 ++++++++++++++ test/runtests.jl | 1 + 7 files changed, 78 insertions(+), 2 deletions(-) create mode 100644 src/environments/controllers/CBF.jl create mode 100644 test/cbf.jl diff --git a/Project.toml b/Project.toml index f929622..5e1edbb 100644 --- a/Project.toml +++ b/Project.toml @@ -1,10 +1,12 @@ name = "FSimZoo" uuid = "e2d5807d-1594-43f7-91a6-54c6eef5a9d2" authors = ["JinraeKim 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" @@ -12,6 +14,8 @@ 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" diff --git a/README.md b/README.md index a51ede8..568bbcd 100644 --- a/README.md +++ b/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` diff --git a/src/FSimZoo.jl b/src/FSimZoo.jl index f2a4585..144f33b 100644 --- a/src/FSimZoo.jl +++ b/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 diff --git a/src/environments/controllers/CBF.jl b/src/environments/controllers/CBF.jl new file mode 100644 index 0000000..402e0d6 --- /dev/null +++ b/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 diff --git a/src/environments/controllers/controllers.jl b/src/environments/controllers/controllers.jl index ac95295..46d928e 100644 --- a/src/environments/controllers/controllers.jl +++ b/src/environments/controllers/controllers.jl @@ -2,3 +2,4 @@ include("BacksteppingPositionController.jl") include("GeometricTrackingController.jl") include("LQR.jl") include("PID.jl") +include("CBF.jl") diff --git a/test/cbf.jl b/test/cbf.jl new file mode 100644 index 0000000..d4ae63d --- /dev/null +++ b/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 diff --git a/test/runtests.jl b/test/runtests.jl index 1615191..f5d2d90 100644 --- a/test/runtests.jl +++ b/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