avoid try/catch and reuse computations in ExplicitSafetyFilter

src/control_affine_system.jl

@@ -10,12 +10,12 @@ Control affine system described by the dynamics ``ẋ = f(x) + g(x)u`` where ``x
 - `f::Function` : drift dynamics
 - `g::Function` : control directions
 """
-struct ControlAffineSystem
+struct ControlAffineSystem{F,G}
     name::String
     n::Int
     m::Int
-    f::Function
-    g::Function
+    f::F
+    g::G
 end
 
 """

src/control_barrier_function.jl

@@ -10,12 +10,12 @@ Control barrier function (CBF) defining a safe set as its zero superlevel set.
 - `Lfh::Function` : Lie derivative of CBF along drift vector field `f`
 - `Lgh::Function` : Lie derivative of CBF along control directions `g`
 """
-struct ControlBarrierFunction
-    h::Function
-    α::Function
-    ∇h::Function
-    Lfh::Function
-    Lgh::Function
+struct ControlBarrierFunction{Th, Tα, T∇h, TLfh, TLgh}
+    h::Th
+    α::Tα
+    ∇h::T∇h
+    Lfh::TLfh
+    Lgh::TLgh
 end
 
 """
@@ -44,4 +44,4 @@ end
 If no extended class K function provided, default to the identify function.
 """
 ControlBarrierFunction(h::Function, Σ::ControlAffineSystem) =
-    ControlBarrierFunction(h, Σ, r -> r)
+    ControlBarrierFunction(h, Σ, identity)

src/explicit_safety_filter.jl

@@ -6,17 +6,16 @@ Controller that uses the closed-form solution to a control barrier function quad
 # Fields
 - `k::Function` : function that computes safe control actions
 """
-struct ExplicitSafetyFilter <: SafetyFilter
-    k::Function
+struct ExplicitSafetyFilter{T} <: SafetyFilter
+    k::T
 end
 
 """
     (k::ExplicitSafetyFilter)(x)
 
 Functors for evaluating explicit safety filter
 """
-(k::ExplicitSafetyFilter)(x) = k.k(x)
-(k::ExplicitSafetyFilter)(x, t) = k.k(x, t)
+(k::ExplicitSafetyFilter)(args...) = k.k(args...)
 
 """
     ExplicitSafetyFilter(cbf::ControlBarrierFunction, Σ::ControlAffineSystem, kd::Function)
@@ -26,23 +25,14 @@ Construct an ExplicitSafetyFilter from a cbf and a desired controller.
 function ExplicitSafetyFilter(
     cbf::ControlBarrierFunction, Σ::ControlAffineSystem, kd::Function
 )
-    try
-        kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0)
-    catch e
-        if isa(e, MethodError)
-            a(x) = cbf.Lfh(x) + cbf.Lgh(x) * kd(x) + cbf.α(cbf(x))
-            k(x) = kd(x) + λQP(a(x), norm(cbf.Lgh(x))^2) * cbf.Lgh(x)'
-
-            return ExplicitSafetyFilter(k)
-        else
-            return e
-        end
-    else
-        a(x, t) = cbf.Lfh(x) + cbf.Lgh(x) * kd(x, t) + cbf.α(cbf(x))
-        k(x, t) = kd(x, t) + λQP(a(x, t), norm(cbf.Lgh(x))^2) * cbf.Lgh(x)'
-
-        return ExplicitSafetyFilter(k)
+    function k(x, args...)
+        Lgh = cbf.Lgh(x)
+        Lfh = cbf.Lfh(x)
+        kdx = kd(x, args...)
+        a = Lfh + Lgh * kdx + cbf.α(cbf(x))
+        kdx + λQP(a, norm(Lgh)^2) * Lgh'
     end
+    ExplicitSafetyFilter{typeof(k)}(k)
 end
 
 """

src/qp_safety_filter.jl

@@ -21,12 +21,12 @@ Functors for evaluating QP-based safety filter
 (k::QPSafetyFilter)(x, t) = k.k(x, t)
 
 """
-    QPSafetyFilter(cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function)
+    QPSafetyFilter(cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function)
 
 Construct an QPSafetyFilter from a cbf and a desired controller.
 """
 function QPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
 )
     try
         kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0) # See if desired controller is time-varying
@@ -43,13 +43,13 @@ end
 
 """
     QPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
 )
 
 Construct an QPSafetyFilter from a cbf and a desired controller.
 """
 function QPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
 )
     try
         kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0) # See if desired controller is time-varying
@@ -80,12 +80,12 @@ function QPSafetyFilter(
 end
 
 """
-    solve_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP using OSQP and JuMP.
 """
 function solve_cbf_qp(
-    x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)
@@ -101,15 +101,15 @@ end
 
 """
     solve_cbf_qp(
-        x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function, umin, umax
+        x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function, umin, umax
     )
 
 Solve CBF-QP using OSQP and JuMP while satisfying input bounds `umin ≤ u ≤ umax`.
 """
 function solve_cbf_qp(
     x,
     Σ::ControlAffineSystem,
-    cbfs::Vector{ControlBarrierFunction},
+    cbfs::Vector{<:ControlBarrierFunction},
     kd::Function,
     umin,
     umax,
@@ -129,12 +129,12 @@ function solve_cbf_qp(
 end
 
 """
-    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP where desired controller is time-varying
 """
 function solve_time_varying_cbf_qp(
-    x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)
@@ -149,15 +149,15 @@ function solve_time_varying_cbf_qp(
 end
 
 """
-    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function, umin, umax)
+    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function, umin, umax)
 
 Solve CBF-QP where desired controller is time-varying with input bounds.
 """
 function solve_time_varying_cbf_qp(
     x,
     t,
     Σ::ControlAffineSystem,
-    cbfs::Vector{ControlBarrierFunction},
+    cbfs::Vector{<:ControlBarrierFunction},
     kd::Function,
     umin,
     umax,

src/tunable_qp_safety_filter.jl

@@ -21,15 +21,15 @@ Functors for evaluating QP-based safety filter
 (k::TunableQPSafetyFilter)(x, t) = k.k(x, t)
 
 """
-    TunableQPSafetyFilter(cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function; tunable=false)
+    TunableQPSafetyFilter(cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function; tunable=false)
 
 Construct an TunableQPSafetyFilter from a cbf and a desired controller.
 
 # Keywork arguments
 - `tunable::Bool` : boolean to decide if coefficients on extended class K functions should be decision variables
 """
 function TunableQPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
 )
     try
         kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0) # See if desired controller is time-varying
@@ -55,12 +55,12 @@ TunableQPSafetyFilter(cbf::ControlBarrierFunction, Σ::ControlAffineSystem, kd::
     TunableQPSafetyFilter([cbf], Σ, kd)
 
 """
-    solve_tunable_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_tunable_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP where coefficients on extended class K functions are decision variables
 """
 function solve_tunable_cbf_qp(
-    x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)
@@ -78,12 +78,12 @@ function solve_tunable_cbf_qp(
 end
 
 """
-    solve_time_varying_tunable_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_time_varying_tunable_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP where coefficients on extended class K functions are decision variables and desired controller depends on time
 """
 function solve_time_varying_tunable_cbf_qp(
-    x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)