Adding unicycle in the new frame

docs/src/examples/solvers/Unicycle robot.jl

@@ -0,0 +1,240 @@
+using Test     #src
+# # Example: Unicycle Robot solved by [Uniform grid abstraction](https://github.com/dionysos-dev/Dionysos.jl/blob/master/docs/src/manual/manual.md#solvers).
+#
+# This example was adapted from the numerical experiments in [1, Sec. 5] from Symbolically guided Model Predictive Control (SgMPC)  [paper](https://doi.org/10.1016/j.ifacol.2022.09.039)..
+# This is a **control problem** for a **discrete-time nonlinear system**.
+#
+# Let us consider the 3-dimensional state space control system of the form
+# ```math
+# x_{t+1} = f(x_t, u_t)
+# ```
+# with $f: \mathbb{R}^3 × \mathbb{R}^2 \to \mathbb{R}^3$ given by
+# ```math
+# f(x, (u_1, u_2)) = \begin{bmatrix} x_1 + u_1 \cos(x_3) \\ x_2 + u_1 \sin(x_3) \\ x_3 + u_2 \ (\text{mod} \ 2\pi) \end{bmatrix}
+# ```
+# and with state and control constraints given by:
+# ```math
+# X = \left\{ (x_1, x_2)^T \in \mathbb{R}^2 \ | \ x_1^2 - x_2^2 \leq 4, \ 4x_2^2 - x_1^2 \leq 16 \right\}
+# ```
+# ```math
+# U = [0.2, 2] \times [-1, 1]
+# ```
+# Here, $(x_1, x_2)$ represents the 2D Cartesian coordinates and $x_3$ is the angular orientation of a mobile cart.
+# The control inputs $u_1$ and $u_2$ are the linear and angular velocities.
+# The control objective is to drive the mobile cart to a desired reference position $x_r$.
+#
+# Considering this as a reachability problem, we will use it to showcase the capabilities of the Uniform grid abstraction solving discrete-time problem in Dionysos.
+# The nonlinear constraints are handled as obstacles in the state-space.
+
+# First, let us import [StaticArrays](https://github.com/JuliaArrays/StaticArrays.jl) and [Plots](https://github.com/JuliaPlots/Plots.jl).
+using StaticArrays, Plots
+
+# At this point, we import Dionysos and JuMP.
+using Dionysos, JuMP
+
+# Define the problem using JuMP
+# We first create a JuMP model:
+model = Model(Dionysos.Optimizer)
+
+# Define the discretization step
+hx = 0.1
+
+# Define the state variables: x1(t), x2(t), x3(t) without specifying the start value since here it's a set. We will specify the start later using constraints.
+x_low = [-3.5, -2.6, -pi]
+x_upp = -x_low
+@variable(model, x_low[i] <= x[i = 1:3] <= x_upp[i])
+
+# Define the control variables: u1(t), u2(t)
+@variable(model, -1 <= u[1:2] <= 1)
+
+# Define the dynamics
+@constraint(model, ∂(x[1]) == x[1] + u[1] * cos(x[3]))
+@constraint(model, ∂(x[2]) == x[2] + u[1] * sin(x[3]))
+@constraint(model, ∂(x[3]) == x[3] + u[2])
+
+# Define the initial and target sets
+x_initial = [1.0, -1.7, 0.0]
+x_target = [sqrt(32) / 3, sqrt(20) / 3, -pi]
+
+@constraint(model, start(x[1]) in MOI.Interval(x_initial[1] - hx, x_initial[1] + hx))
+@constraint(model, start(x[2]) in MOI.Interval(x_initial[2] - hx, x_initial[2] + hx))
+@constraint(model, start(x[3]) in MOI.Interval(x_initial[3] - hx, x_initial[3] + hx))
+
+@constraint(model, final(x[1]) in MOI.Interval(x_target[1] - hx, x_target[1] + hx))
+@constraint(model, final(x[2]) in MOI.Interval(x_target[2] - hx, x_target[2] + hx))
+@constraint(model, final(x[3]) in MOI.Interval{Float64}(-pi, pi))
+
+# Obstacle boundaries computed using the function `get_obstacles` below
+function extract_rectangles(matrix)
+    if isempty(matrix)
+        return []
+    end
+
+    n, m = size(matrix)
+    tlx, tly, brx, bry = Int[], Int[], Int[], Int[]
+
+    # Build histogram heights
+    for i in 1:n
+        j = 1
+        while j <= m
+            if matrix[i, j] == 1
+                j += 1
+                continue
+            end
+            push!(tlx, j)
+            push!(tly, i)
+            while j <= m && matrix[i, j] == 0
+                j += 1
+            end
+            push!(brx, j - 1)
+            push!(bry, i)
+        end
+    end
+
+    return zip(tlx, tly, brx, bry)
+end
+
+function get_obstacles(lb, ub, h)
+    ## lb_x1 = -3.5, ub_x1 = 3.5, lb_x2 = -2.6, ub_x2 = 2.6, h = 0.1
+    lb_x1, lb_x2, lb_x3 = lb
+    ub_x1, ub_x2, ub_x3 = ub
+
+    ## Define the obstacles
+    x1 = range(lb_x1; stop = ub_x1, step = h)
+    x2 = range(lb_x2; stop = ub_x2, step = h)
+    steps1, steps2 = length(x1), length(x2)
+
+    X1 = x1' .* ones(steps2)
+    X2 = ones(steps1)' .* x2
+
+    Z1 = (X1 .^ 2 .- X2 .^ 2) .<= 4
+    Z2 = (4 .* X2 .^ 2 .- X1 .^ 2) .<= 16
+
+    ## Find the upper and lower bounds of X1 and X2 for the obstacle 
+    grid = Z1 .& Z2
+
+    return [
+        MOI.HyperRectangle([x1[x1lb], x2[x2lb]], [x1[x1ub], x2[x2ub]]) for
+        (x1lb, x2lb, x1ub, x2ub) in extract_rectangles(grid)
+    ]
+end
+
+obstacles = get_obstacles(x_low, x_upp, hx)
+
+# Function to add rectangular obstacle avoidance constraints
+
+for obstacle in obstacles
+    @constraint(model, x[1:2] ∉ obstacle)
+end
+
+# ### Definition of the abstraction
+
+# First we need to set the mode of the abstraction to `DIONYSOS.Optim.Abstraction.UniformGridAbstraction.DSICRETE_TIME`:
+set_attribute(
+    model,
+    "approx_mode",
+    Dionysos.Optim.Abstraction.UniformGridAbstraction.DSICRETE_TIME,
+)
+
+# We define the system map $f$:
+function sys_map(x, u, _)
+    return StaticArrays.SVector{3}(
+        x[1] + u[1] * cos(x[3]),
+        x[2] + u[1] * sin(x[3]),
+        (x[3] + u[2]) % (2 * π),
+    )
+end
+set_attribute(model, "system_map", sys_map)
+
+# We define the growth bound function of $f$:
+function growth_bound(r, u, _)
+    β = u[1] * r[3]
+    return StaticArrays.SVector{3}(β, β, 0.0)
+end
+set_attribute(model, "growthbound_map", growth_bound)
+
+# We define the invariant system map:
+function sys_inv(x, u, _)
+    return StaticArrays.SVector{3}(
+        x[1] - u[1] * cos((x[3] - u[2]) % (2 * π)),
+        x[2] - u[1] * sin((x[3] - u[2]) % (2 * π)),
+        (x[3] - u[2]) % (2 * π),
+    )
+end
+set_attribute(model, "sys_inv_map", sys_inv)
+
+# We define the time step of the system:
+set_attribute(model, "time_step", 1.0)
+
+# Definition of the grid of the state-space on which the abstraction is based (origin `x0` and state-space discretization `h`):
+x0 = SVector(0.0, 0.0, 0.0);
+h = SVector(hx, hx, 0.2);
+set_attribute(model, "state_grid", Dionysos.Domain.GridFree(x0, h))
+
+# Definition of the grid of the input-space on which the abstraction is based (origin `u0` and input-space discretization `h`):
+u0 = SVector(1.1, 0.0);
+h = SVector(0.3, 0.3);
+set_attribute(model, "input_grid", Dionysos.Domain.GridFree(u0, h))
+
+optimize!(model)
+
+# Get the results
+abstract_system = get_attribute(model, "abstract_system");
+abstract_problem = get_attribute(model, "abstract_problem");
+abstract_controller = get_attribute(model, "abstract_controller");
+concrete_controller = get_attribute(model, "concrete_controller")
+concrete_problem = get_attribute(model, "concrete_problem");
+concrete_system = concrete_problem.system
+
+# ### Trajectory display
+# We choose a stopping criterion `reached` and the maximal number of steps `nsteps` for the sampled system, i.e. the total elapsed time: `nstep`*`tstep`
+# as well as the true initial state `x0` which is contained in the initial state-space `_I_` defined previously.
+nstep = 100
+function reached(x)
+    if x ∈ concrete_problem.target_set
+        return true
+    else
+        return false
+    end
+end
+
+control_trajectory = Dionysos.System.get_closed_loop_trajectory(
+    get_attribute(model, "discretized_system"),
+    concrete_controller,
+    x_initial,
+    nstep;
+    stopping = reached,
+)
+
+using Plots
+
+# Here we display the coordinate projection on the two first components of the state space along the trajectory.
+fig = plot(; aspect_ratio = :equal);
+# We display the concrete domain
+plot!(concrete_system.X; color = :yellow, opacity = 0.5);
+
+# We display the abstract domain
+plot!(abstract_system.Xdom; color = :blue, opacity = 0.5);
+
+# We display the concrete specifications
+plot!(concrete_problem.initial_set; color = :green, opacity = 0.2);
+plot!(concrete_problem.target_set; dims = [1, 2], color = :red, opacity = 0.2);
+
+# We display the abstract specifications
+plot!(
+    Dionysos.Symbolic.get_domain_from_symbols(
+        abstract_system,
+        abstract_problem.initial_set,
+    );
+    color = :green,
+);
+plot!(
+    Dionysos.Symbolic.get_domain_from_symbols(abstract_system, abstract_problem.target_set);
+    color = :red,
+);
+
+# We display the concrete trajectory
+plot!(control_trajectory; ms = 0.5)
+
+# ### References
+# 1. Z. Azaki, A. Girard and S. Olaru, "Predictive and Symbolic Control: Performance and Safety for Non-linear Systems," in IFAC-PapersOnLine, 2022, vol. 55, no 16, pp. 290-295..

src/MOI_wrapper.jl

@@ -17,7 +17,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer
     variable_index::Vector{Int} # MOI.VariableIndex -> state/input/mode index
     lower::Vector{Float64}
     upper::Vector{Float64}
-    start::Vector{Float64}
+    start::Vector{MOI.Interval{Float64}}
     target::Vector{MOI.Interval{Float64}}
     dynamic::Vector{Union{Nothing, MOI.ScalarNonlinearFunction}}
     obstacles::Vector{Tuple{Vector{MOI.VariableIndex}, MOI.HyperRectangle}}
@@ -35,7 +35,7 @@ mutable struct Optimizer <: MOI.AbstractOptimizer
             Int[],
             Float64[],
             Float64[],
-            Float64[],
+            MOI.Interval{Float64}[],
             MOI.Interval{Float64}[],
             Union{Nothing, MOI.ScalarNonlinearFunction}[],
             Tuple{Vector{MOI.VariableIndex}, MOI.HyperRectangle}[],
@@ -71,7 +71,7 @@ function MOI.add_variable(model::Optimizer)
     push!(model.variable_index, 0)
     push!(model.lower, -Inf)
     push!(model.upper, Inf)
-    push!(model.start, NaN)
+    push!(model.start, MOI.Interval(-Inf, Inf))
     push!(model.target, MOI.Interval(-Inf, Inf))
     push!(model.dynamic, nothing)
     return MOI.VariableIndex(length(model.upper))
@@ -139,6 +139,12 @@ function MOI.add_constraint(
             model.nonlinear_index += 1
             return MOI.ConstraintIndex{typeof(func), typeof(set)}(model.nonlinear_index)
         end
+
+        if var isa MOI.VariableIndex && func.head == :start
+            model.start[var.value] = set
+            model.nonlinear_index += 1
+            return MOI.ConstraintIndex{typeof(func), typeof(set)}(model.nonlinear_index)
+        end
     end
     dump(func)
     return error("Unsupported")
@@ -191,6 +197,11 @@ function MOI.supports(::Optimizer, ::MOI.VariablePrimalStart, ::Type{MOI.Variabl
 end
 
 function MOI.set(model::Optimizer, ::MOI.VariablePrimalStart, vi::MOI.VariableIndex, value)
+    # create a MOI.Interval from the value if value is a scalar
+    if !isa(value, MOI.Interval)
+        value = MOI.Interval(value, value)
+    end
+
     return model.start[vi.value] = value
 end
 
@@ -304,8 +315,10 @@ end
 function problem(model::Optimizer)
     x_idx = state_indices(model)
     u_idx = input_indices(model)
-    _I_ =
-        Dionysos.Utils.HyperRectangle(_svec(model.start, x_idx), _svec(model.start, x_idx))
+    _I_ = Dionysos.Utils.HyperRectangle(
+        _svec([s.lower for s in model.start], x_idx),
+        _svec([s.upper for s in model.start], x_idx),
+    )
     _T_ = Dionysos.Utils.HyperRectangle(
         _svec([s.lower for s in model.target], x_idx),
         _svec([s.upper for s in model.target], x_idx),
@@ -387,14 +400,17 @@ function MOI.set(model::Optimizer, attr::MOI.RawOptimizerAttribute, value)
     return MOI.set(model.inner, attr, value)
 end
 
-export ∂, final
+export ∂, final, start
 
 function _diff end
 ∂ = JuMP.NonlinearOperator(_diff, :diff)
 
 function _final end
 final = JuMP.NonlinearOperator(_final, :final)
 
+function _start end
+start = JuMP.NonlinearOperator(_start, :start)
+
 # Type piracy
 function JuMP.parse_constraint_call(
     error_fn::Function,

src/optim/abstraction/uniform_grid_abstraction.jl

@@ -12,7 +12,7 @@ const ST = DI.System
 const SY = DI.Symbolic
 const PR = DI.Problem
 
-@enum ApproxMode GROWTH LINEARIZED DELTA_GAS
+@enum ApproxMode GROWTH LINEARIZED DELTA_GAS DSICRETE_TIME
 
 using JuMP
 
@@ -37,6 +37,13 @@ mutable struct Optimizer{T} <: MOI.AbstractOptimizer
     δGAS::Union{Nothing, Bool}
     solve_time_sec::T
     approx_mode::ApproxMode
+
+    ## for the discrete time (no need for the jacobian_bound) 
+    #but for the system_map, growthbound_map and sys_inv_map
+    system_map::Union{Nothing, Function}
+    growthbound_map::Union{Nothing, Function}
+    sys_inv_map::Union{Nothing, Function}
+
     function Optimizer{T}() where {T}
         return new{T}(
             nothing,
@@ -54,6 +61,9 @@ mutable struct Optimizer{T} <: MOI.AbstractOptimizer
             false,
             0.0,
             GROWTH,
+            nothing,
+            nothing,
+            nothing,
         )
     end
 end
@@ -100,6 +110,24 @@ function build_abstraction(concrete_system, model)
             model.num_sub_steps_system_map,
             model.num_sub_steps_growth_bound,
         )
+    elseif model.approx_mode == DSICRETE_TIME
+        if isnothing(model.system_map)
+            error("Please set the `system_map`.")
+        end
+        if isnothing(model.growthbound_map)
+            error("Please set the `growthbound_map`.")
+        end
+        if isnothing(model.sys_inv_map)
+            error("Please set the `sys_inv_map`.")
+        end
+        Dionysos.System.ControlSystemGrowth(
+            model.time_step,
+            noise,
+            noise,
+            model.system_map,
+            model.growthbound_map,
+            model.sys_inv_map,
+        )
     elseif model.approx_mode == LINEARIZED
         Dionysos.System.NewControlSystemLinearizedRK4(
             model.time_step,