Test PiecewiseNonlinearAdditiveNoiseDynamics

Author: Zinoex

src/dynamics/PiecewiseNonlinearAdditiveNoiseDynamics.jl

@@ -5,16 +5,23 @@ export PiecewiseNonlinearAdditiveNoiseDynamics, NonlinearDynamicsRegion
 
 A struct representing a non-linear dynamics, valid over a region.
 """
-struct NonlinearDynamicsRegion{F<:Function, S<:LazySet}
+struct NonlinearDynamicsRegion{F<:Function,S<:LazySet}
     f::F
     region::S
 end
 
-function (dyn::NonlinearDynamicsRegion)(X::Hyperrectangle{Float64}, U::Hyperrectangle{Float64})
+function (dyn::NonlinearDynamicsRegion)(
+    X::Hyperrectangle{Float64},
+    U::Hyperrectangle{Float64},
+)
     # Use the Taylor model to over-approximate the reachable set
     active_region = intersection(X, dyn.region)
     if isempty(active_region)
-        throw(ArgumentError("The input region X does not intersect with the valid region of the dynamics"))
+        throw(
+            ArgumentError(
+                "The input region X does not intersect with the valid region of the dynamics",
+            ),
+        )
     end
     active_region_box = box_approximation(active_region)
 
@@ -28,7 +35,10 @@ function (dyn::NonlinearDynamicsRegion)(X::Hyperrectangle{Float64}, U::Hyperrect
     # set_variables(Float64, "z"; order=order, numvars=LazySets.dim(X) + LazySets.dim(U))
 
     order = 1
-    z = [TaylorModelN(i, order, IntervalBox(z0), dom) for i = 1:LazySets.dim(X)+LazySets.dim(U)]
+    z = [
+        TaylorModelN(i, order, IntervalBox(z0), dom) for
+        i = 1:LazySets.dim(X)+LazySets.dim(U)
+    ]
     x, u = (z-z0)[1:LazySets.dim(X)], z[LazySets.dim(X)+1:end]
 
     # Perform the Taylor expansion
@@ -57,7 +67,11 @@ function (dyn::NonlinearDynamicsRegion)(X::Hyperrectangle{Float64}, U::Hyperrect
     Dupper = sup.(D)
 
     Y1 = Alow * Translation(active_region, -x0) + Blow * Translation(U, -u0) + Clow + Dlower
-    Y2 = Aupper * Translation(active_region, -x0) + Bupper * Translation(U, -u0) + Cupper + Dupper
+    Y2 =
+        Aupper * Translation(active_region, -x0) +
+        Bupper * Translation(U, -u0) +
+        Cupper +
+        Dupper
 
     Yconv = ConvexHull(Y1, Y2)
 
@@ -69,12 +83,19 @@ function (dyn::NonlinearDynamicsRegion)(X::Hyperrectangle{Float64}, U::Singleton
     return dyn(X, element(U))
 end
 
-function (dyn::NonlinearDynamicsRegion)(X::Hyperrectangle{Float64}, u::AbstractVector{Float64})
+function (dyn::NonlinearDynamicsRegion)(
+    X::Hyperrectangle{Float64},
+    u::AbstractVector{Float64},
+)
     # Use the Taylor model to over-approximate the reachable set
 
     active_region = intersection(X, dyn.region)
     if isempty(active_region)
-        throw(ArgumentError("The input region X does not intersect with the valid region of the dynamics"))
+        throw(
+            ArgumentError(
+                "The input region X does not intersect with the valid region of the dynamics",
+            ),
+        )
     end
     active_region_box = box_approximation(active_region)
 
@@ -123,9 +144,16 @@ function (dyn::NonlinearDynamicsRegion)(X::Singleton{Float64}, U::Singleton{Floa
     return Singleton(y)
 end
 
-function (dyn::NonlinearDynamicsRegion)(x::AbstractVector{Float64}, u::AbstractVector{Float64})
+function (dyn::NonlinearDynamicsRegion)(
+    x::AbstractVector{Float64},
+    u::AbstractVector{Float64},
+)
     if x ∉ dyn.region
-        throw(ArgumentError("The input x does not belong to the valid region of the dynamics"))
+        throw(
+            ArgumentError(
+                "The input x does not belong to the valid region of the dynamics",
+            ),
+        )
     end
 
     return dyn.f(x, u)
@@ -220,7 +248,7 @@ function nominal(
     dyn::PiecewiseNonlinearAdditiveNoiseDynamics,
     x::AbstractVector{Float64},
     u,
-) 
+)
     for region in dyn.regions
         if x ∈ region.region
             return region(x, u)
@@ -243,4 +271,4 @@ function prepare_nominal(dyn::PiecewiseNonlinearAdditiveNoiseDynamics, input_abs
     set_variables(Float64, "z"; order = 2, numvars = n)
 
     return nothing
-end
+end

test/dynamics/dynamics.jl

@@ -6,6 +6,7 @@ test_files = [
     "uncertain_pwa.jl",
     "gaussian_process.jl",
     "stochastic_switched.jl",
+    "piecewise_nonlinear.jl",
 ]
 for f in test_files
     @testset "dynamics/$f" include(f)

test/dynamics/piecewise_nonlinear.jl

@@ -0,0 +1,106 @@
+using Revise, Test
+using IntervalMDPAbstractions, LazySets
+
+τ = 0.1
+
+region1 = Hyperrectangle(low = [-1.0, -1.0], high = [0.0, 1.0])
+f(x, u) = [x[1] + x[2] * τ, x[2] + (-x[1] + (1 - x[1])^2 * x[2]) * τ]
+dyn_reg1 = NonlinearDynamicsRegion(f, region1)
+
+region2 = Hyperrectangle(low = [0.0, -1.0], high = [1.0, 1.0])
+g(x, u) = [x[1] + x[2] * τ, x[2] + (-x[2] + (1 - x[2])^2 * x[1]) * τ]
+dyn_reg2 = NonlinearDynamicsRegion(g, region2)
+
+w_stddev = [0.1, 0.1]
+w = AdditiveDiagonalGaussianNoise(w_stddev)
+
+dyn = PiecewiseNonlinearAdditiveNoiseDynamics([dyn_reg1, dyn_reg2], 2, 0, w)
+
+prepare_nominal(dyn, InputDiscrete([0.0]))
+
+X = Hyperrectangle(low = [-1.0, 0.0], high = [1.0, 1.0])
+U = Singleton([0.0])
+
+Y = concretize(nominal(dyn, X, U))
+
+# First piecewise region
+X1 = Hyperrectangle(low = [-1.0, 0.0], high = [0.0, 1.0])
+
+# 1st-order Taylor expansion at [center(X1)] = [-0.5; 0.5]:  
+# z = x - center(X1)
+# y₁ = z₁ + 0.1 * z₂ - 0.45
+# y₂ = -0.25 * z₁ + 1.2245 * z₂ + 0.6625 + [-0.075, 0.1]
+X1centered = Translation(X1, -[-0.5, 0.5])
+AXD1 = AffineMap([1.0 0.1; -0.25 1.225], X1centered, [-0.45, 0.6625])
+Y1_expected = MinkowskiSum(AXD1, Hyperrectangle(low = [0.0, -0.075], high = [0.0, 0.1]))
+
+# First piecewise region
+X2 = Hyperrectangle(low = [0.0, 0.0], high = [1.0, 1.0])
+
+# 1st-order Taylor expansion at [center(X2)] = [0.5; 0.5]:
+# z = x - center(X2)
+# y₁ = z₁ + 0.1 * z₂ + 0.55
+# y₂ = 0.025 * z₁ + 0.85 * z₂ + 0.4625 + [-0.025, 0.05]
+X2centered = Translation(X2, -[0.5, 0.5])
+AXD2 = AffineMap([1.0 0.1; 0.025 0.85], X2centered, [0.55, 0.4625])
+Y2_expected = MinkowskiSum(AXD2, Hyperrectangle(low = [0.0, -0.025], high = [0.0, 0.05]))
+
+Y_expected = concretize(ConvexHull(Y1_expected, Y2_expected))
+@test isequivalent(Y, Y_expected)
+
+
+#### Region-based actions
+region2 = Hyperrectangle(low = [0.0, -1.0], high = [1.0, 1.0])
+h(x, u) = [x[1] + x[2] * τ, x[2] + (-x[2] + (1 - x[2])^2 * x[1] + u[1]) * τ]
+dyn_reg2 = NonlinearDynamicsRegion(h, region2)
+
+w_stddev = [0.1, 0.1]
+w = AdditiveDiagonalGaussianNoise(w_stddev)
+
+dyn = PiecewiseNonlinearAdditiveNoiseDynamics([dyn_reg1, dyn_reg2], 2, 1, w)
+
+input_abs = InputGridSplit(Hyperrectangle(low = [-1.0], high = [1.0]), (3,))
+prepare_nominal(dyn, input_abs)
+
+X = Hyperrectangle(low = [-1.0, 0.0], high = [1.0, 1.0])
+U = Hyperrectangle(low = [-1.0], high = [-1 / 3])
+
+Y = concretize(nominal(dyn, X, U))
+
+# First piecewise region
+X1 = Hyperrectangle(low = [-1.0, 0.0], high = [0.0, 1.0])
+
+# 1st-order Taylor expansion at [center(X1)] = [-0.5; 0.5]:  
+# z = x - center(X1)
+# y₁ = z₁ + 0.1 * z₂
+# y₂ = -0.1 * z₁ + 1.1 * z₂ + 0.6625 + [-0.075, 0.1]
+X1centered = Translation(X1, -[-0.5, 0.5])
+AXD1 = LinearMap([1.0 0.1; -0.1 1.1], X1centered)
+Y1_expected =
+    MinkowskiSum(AXD1, Hyperrectangle(; low = [0.0, -0.0625], high = [0.0, 0.0625]))
+
+# First piecewise region
+X2 = Hyperrectangle(low = [0.0, 0.0], high = [1.0, 1.0])
+
+# 1st-order Taylor expansion at [center(X2)] = [0.5; 0.5]:
+# z = x - center(X2)
+# y₁ = z₁ + 0.1 * z₂
+# y₂ = 0.1 * z₁ + 0.9 * z₂ + 0.1 * u₁ - 1/15 + [-0.0625, 0.0625]
+X2centered = Translation(X2, -[0.5, 0.5])
+AXD2 = AffineMap([1.0 0.1; 0.1 0.9], X2centered, [0.0, -1 / 15])
+U = Hyperrectangle(low = [-1.0], high = [-1 / 3])
+Ucentered = Translation(U, -center(U))
+AXDBU2 = MinkowskiSum(AXD2, LinearMap([0.0; 0.1], Ucentered))
+Y2_expected =
+    MinkowskiSum(AXDBU2, Hyperrectangle(low = [0.0, -0.0625], high = [0.0, 0.0625]))
+
+Y_expected = concretize(ConvexHull(Y1_expected, Y2_expected))
+@test isequivalent(Y, Y_expected)
+
+# Vector inputs
+x = [0.5, 0.5]
+u = [0.0]
+
+y = nominal(dyn, x, u)
+y_expected = g(x, u)
+@test y ≈ y_expected