Merge pull request #124 from control-toolbox/108-dev-coordinate-wise-dynamics

Coordinate wise dynamics

Author: ocots

Project.toml

@@ -28,7 +28,7 @@ CTModelsPlots = "Plots"
 [compat]
 CTBase = "0.16"
 DocStringExtensions = "0.9"
-Interpolations = "0.15"
+Interpolations = "0.16"
 JLD2 = "0.5"
 JSON3 = "1.14"
 LinearAlgebra = "1"

docs/Project.toml

@@ -4,4 +4,5 @@ Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 
 [compat]
 Documenter = "1"
+Plots = "1.40"
 julia = "1.10"

src/constraints.jl

@@ -62,14 +62,14 @@ function __constraint!(
 )
 
     # checkings: the constraint must not be set before
-    label ∈ keys(ocp_constraints) && throw(
+    @ensure(!(label ∈ keys(ocp_constraints)),
         CTBase.UnauthorizedCall(
             "the constraint named " * String(label) * " already exists."
         ),
     )
 
     # checkings: lb and ub cannot be both nothing
-    (isnothing(lb) && isnothing(ub)) && throw(
+    @ensure(!(isnothing(lb) && isnothing(ub)),
         CTBase.UnauthorizedCall(
             "The lower bound `lb` and the upper bound `ub` cannot be both nothing."
         ),
@@ -80,7 +80,7 @@ function __constraint!(
     isnothing(ub) && (ub = Inf * ones(eltype(lb), length(lb)))
 
     # lb and ub must have the same length
-    length(lb) != length(ub) && throw(
+    @ensure(length(lb) == length(ub),
         CTBase.IncorrectArgument(
             "the lower bound `lb` and the upper bound `ub` must have the same length."
         ),
@@ -107,28 +107,28 @@ function __constraint!(
                     ),
                 )
             end
-            (length(rg) != length(lb)) && throw(CTBase.IncorrectArgument(txt))
+            @ensure(length(rg) == length(lb), CTBase.IncorrectArgument(txt))
             __constraint!(ocp_constraints, type, n, m, q; rg=rg, lb=lb, ub=ub, label=label)
         end
 
         (::OrdinalRange{<:Int}, ::Nothing, ::ctVector, ::ctVector) => begin
             txt = "the range `rg`, the lower bound `lb` and the upper bound `ub` must have the same dimension"
-            (length(rg) != length(lb)) && throw(CTBase.IncorrectArgument(txt))
+            @ensure(length(rg) == length(lb), CTBase.IncorrectArgument(txt))
             # check if the range is valid
             if type == :state
-                !all(1 .≤ rg .≤ n) && throw(
+                @ensure(all(1 .≤ rg .≤ n),
                     CTBase.IncorrectArgument(
                         "the range of the state constraint must be contained in 1:$n",
                     ),
                 )
             elseif type == :control
-                !all(1 .≤ rg .≤ m) && throw(
+                @ensure(all(1 .≤ rg .≤ m),
                     CTBase.IncorrectArgument(
                         "the range of the control constraint must be contained in 1:$m",
                     ),
                 )
             elseif type == :variable
-                !all(1 .≤ rg .≤ q) && throw(
+                @ensure(all(1 .≤ rg .≤ q),
                     CTBase.IncorrectArgument(
                         "the range of the variable constraint must be contained in 1:$q",
                     ),
@@ -192,50 +192,44 @@ julia> constraint!(ocp, :control, rg=1:2, lb=[0.0], ub=[1.0], label=:control_con
 function constraint!(
     ocp::PreModel,
     type::Symbol;
-    rg::Union{Int,OrdinalRange{Int},Nothing}=nothing,
-    f::Union{Function,Nothing}=nothing,
-    lb::Union{ctNumber,ctVector,Nothing}=nothing,
-    ub::Union{ctNumber,ctVector,Nothing}=nothing,
-    label::Symbol=__constraint_label(),
+    rg::Union{Int, OrdinalRange{Int}, Nothing} = nothing,
+    f::Union{Function, Nothing} = nothing,
+    lb::Union{ctNumber, ctVector, Nothing} = nothing,
+    ub::Union{ctNumber, ctVector, Nothing} = nothing,
+    label::Symbol = __constraint_label(),
 )
 
     # checkings: times, state and control must be set before adding constraints
-    !__is_state_set(ocp) &&
-        throw(CTBase.UnauthorizedCall("the state must be set before adding constraints."))
-    !__is_control_set(ocp) &&
-        throw(CTBase.UnauthorizedCall("the control must be set before adding constraints."))
-    !__is_times_set(ocp) &&
-        throw(CTBase.UnauthorizedCall("the times must be set before adding constraints."))
-
-    # checkings: if the ocp has no variable, then the constraint! function cannot be used with type=:variable
-    !__is_variable_set(ocp) &&
-        type == :variable &&
-        throw(
-            CTBase.UnauthorizedCall(
-                "the ocp has no variable" *
-                ", you cannot use constraint! function with type=:variable. If it is a mistake, please set the variable first.",
-            ),
-        )
+    @ensure __is_state_set(ocp) CTBase.UnauthorizedCall("the state must be set before adding constraints.")
+    @ensure __is_control_set(ocp) CTBase.UnauthorizedCall("the control must be set before adding constraints.")
+    @ensure __is_times_set(ocp) CTBase.UnauthorizedCall("the times must be set before adding constraints.")
+
+    # checkings: variable must be set if using type=:variable
+    @ensure (type != :variable || __is_variable_set(ocp)) CTBase.UnauthorizedCall(
+        "the ocp has no variable, you cannot use constraint! function with type=:variable. If it is a mistake, please set the variable first."
+    )
 
     # dimensions
     n = dimension(ocp.state)
     m = dimension(ocp.control)
     q = dimension(ocp.variable)
+
     # add the constraint
     return __constraint!(
         ocp.constraints,
         type,
         n,
         m,
         q;
-        rg=as_range(rg),
-        f=f,
-        lb=as_vector(lb),
-        ub=as_vector(ub),
-        label=label,
+        rg = as_range(rg),
+        f = f,
+        lb = as_vector(lb),
+        ub = as_vector(ub),
+        label = label,
     )
 end
 
+
 as_vector(::Nothing) = nothing
 (as_vector(x::T)::Vector{T}) where {T<:ctNumber} = [x]
 as_vector(x::Vector{T}) where {T<:ctNumber} = x

src/control.jl

@@ -42,32 +42,24 @@ julia> control_components(ocp)
 function control!(
     ocp::PreModel,
     m::Dimension,
-    name::T1=__control_name(),
-    components_names::Vector{T2}=__control_components(m, string(name)),
-)::Nothing where {T1<:Union{String,Symbol},T2<:Union{String,Symbol}}
-
-    # checkings
-    __is_control_set(ocp) &&
-        throw(CTBase.UnauthorizedCall("the control has already been set."))
-
-    (m > 0) &&
-        (size(components_names, 1) ≠ m) &&
-        throw(
-            CTBase.IncorrectArgument(
-                "the number of control names must be equal to the control dimension"
-            ),
-        )
-
-    # if the dimension is 0 then throw an error
-    (m == 0) &&
-        throw(CTBase.IncorrectArgument("the control dimension must be greater than 0"))
+    name::T1 = __control_name(),
+    components_names::Vector{T2} = __control_components(m, string(name)),
+)::Nothing where {T1<:Union{String,Symbol}, T2<:Union{String,Symbol}}
+
+    # checkings using @ensure
+    @ensure !__is_control_set(ocp) CTBase.UnauthorizedCall("the control has already been set.")
+    @ensure m > 0 CTBase.IncorrectArgument("the control dimension must be greater than 0")
+    @ensure size(components_names, 1) == m CTBase.IncorrectArgument(
+        "the number of control names must be equal to the control dimension"
+    )
 
     # set the control
     ocp.control = ControlModel(string(name), string.(components_names))
 
     return nothing
 end
 
+
 # ------------------------------------------------------------------------------ #
 # GETTERS
 # ------------------------------------------------------------------------------ #

src/dual_model.jl

@@ -107,8 +107,9 @@ function dual(sol::Solution, model::Model, label::Symbol)
         end
     end
 
-    # return an exception if the label is not found
-    return CTBase.IncorrectArgument("Label $label not found in the model.")
+    # throw an exception if the label is not found
+    throw(CTBase.IncorrectArgument("Label $label not found in the model."))
+    
 end
 
 """