From ee40db06c8a9b855957135347e120917e92fcacb Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 19 Mar 2026 08:26:25 +0000 Subject: [PATCH 1/5] Initial plan From 31e977342ae3e652d3e3165435b60ab6e7014c7d Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 19 Mar 2026 09:20:49 +0000 Subject: [PATCH 2/5] Add JuMP-like macros: @control, @state, @output, @disturbance, @constraint, @objective Co-authored-by: darnstrom <55484604+darnstrom@users.noreply.github.com> --- src/LinearMPC.jl | 4 + src/macros.jl | 442 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 446 insertions(+) create mode 100644 src/macros.jl diff --git a/src/LinearMPC.jl b/src/LinearMPC.jl index cc181d3d..d2a60a4f 100644 --- a/src/LinearMPC.jl +++ b/src/LinearMPC.jl @@ -50,6 +50,10 @@ include("observer.jl"); export predict_state!,correct_state! export set_state!,get_state,update_state! +include("macros.jl"); +export ControlRef, StateRef, OutputRef, DisturbanceRef, SignalExpr +export @control, @state, @output, @disturbance, @constraint, @objective + using PrecompileTools @setup_workload begin diff --git a/src/macros.jl b/src/macros.jl new file mode 100644 index 00000000..8e1e0d69 --- /dev/null +++ b/src/macros.jl @@ -0,0 +1,442 @@ +# ============================================================================ +# JuMP-like macro interface for LinearMPC +# ============================================================================ +# +# Usage example: +# +# mpc = MPC(F, G; C=C, Np=10) +# @control mpc u umin=-ones(2) umax=ones(2) +# @state mpc x +# @output mpc y +# @disturbance mpc d wmin=-0.1 wmax=0.1 +# @objective mpc Q=I R=0.1*I Rr=0.01*I +# @constraint mpc -1 <= u <= 1 +# @constraint mpc 0 <= y <= 5 +# @constraint mpc lb <= Au*u + Ax*x <= ub # general linear constraint +# @constraint mpc u <= 0.5 +# @constraint mpc -1 <= u <= 1 soft=true +# setup!(mpc) + +# ---- Signal reference types ------------------------------------------------ + +""" + ControlRef(mpc) + +Reference to the control input signal of an MPC controller. +Created by the [`@control`](@ref) macro for use in [`@constraint`](@ref) expressions. +""" +struct ControlRef + mpc::MPC +end + +""" + StateRef(mpc) + +Reference to the state signal of an MPC controller. +Created by the [`@state`](@ref) macro for use in [`@constraint`](@ref) expressions. +""" +struct StateRef + mpc::MPC +end + +""" + OutputRef(mpc) + +Reference to the output signal of an MPC controller. +Created by the [`@output`](@ref) macro for use in [`@constraint`](@ref) expressions. +""" +struct OutputRef + mpc::MPC +end + +""" + DisturbanceRef(mpc) + +Reference to the disturbance signal of an MPC controller. +Created by the [`@disturbance`](@ref) macro for use in [`@constraint`](@ref) expressions. +""" +struct DisturbanceRef + mpc::MPC +end + +# ---- SignalExpr: weighted combination of signals --------------------------- + +# Internal: A * [signal_kind] term +struct SignalTerm + A::Matrix{Float64} + kind::Symbol # :u, :x, :r, :d, :uprev +end + +""" + SignalExpr(mpc, terms) + +A linear combination of weighted MPC signals (e.g. `Au*u + Ax*x`), +used inside [`@constraint`](@ref) expressions. + +Built automatically when multiplying a matrix by a signal reference: +```julia +Au*u + Ax*x # where u::ControlRef, x::StateRef +``` +""" +struct SignalExpr + mpc::MPC + terms::Vector{SignalTerm} +end + +# ---- Arithmetic overloads -------------------------------------------------- + +""" + A * u where u::ControlRef + +Produce a weighted control signal expression for use in `@constraint`. +""" +Base.:*(A::AbstractMatrix, u::ControlRef) = + SignalExpr(u.mpc, [SignalTerm(float(A), :u)]) + +""" + A * x where x::StateRef + +Produce a weighted state signal expression for use in `@constraint`. +""" +Base.:*(A::AbstractMatrix, x::StateRef) = + SignalExpr(x.mpc, [SignalTerm(float(A), :x)]) + +""" + A * d where d::DisturbanceRef + +Produce a weighted disturbance signal expression for use in `@constraint`. +""" +Base.:*(A::AbstractMatrix, d::DisturbanceRef) = + SignalExpr(d.mpc, [SignalTerm(float(A), :d)]) + +"""Combine two `SignalExpr` values (sum).""" +function Base.:+(e1::SignalExpr, e2::SignalExpr) + @assert e1.mpc === e2.mpc "Signal expressions must refer to the same MPC controller" + SignalExpr(e1.mpc, [e1.terms; e2.terms]) +end + +# ---- Internal helpers ------------------------------------------------------ + +# Convert `nothing` → empty Float64 vector (for optional bound arguments) +_bound(::Nothing) = zeros(0) +_bound(x) = x + +# Expand a bound to a vector of length `n` when a scalar is provided. +# Useful so that `@constraint mpc 0 <= y <= 5` works for any ny. +_to_vec_bound(::Nothing, n) = zeros(0) +_to_vec_bound(x::AbstractVector, n) = x +_to_vec_bound(x::Number, n) = fill(Float64(x), n) + +# Check whether a value is a signal reference (for one-sided constraint dispatch) +_is_signal(::ControlRef) = true +_is_signal(::OutputRef) = true +_is_signal(::StateRef) = true +_is_signal(::DisturbanceRef) = true +_is_signal(::SignalExpr) = true +_is_signal(::Any) = false + +# ---- Core constraint dispatch ---------------------------------------------- + +# Control: lb ≤ u ≤ ub → set_input_bounds! +function _add_constraint!(mpc::MPC, lb, ::ControlRef, ub; kw...) + set_input_bounds!(mpc; umin=_bound(lb), umax=_bound(ub)) +end + +# Output: lb ≤ y ≤ ub → set_output_bounds! +function _add_constraint!(mpc::MPC, lb, ::OutputRef, ub; + ks=2:mpc.Np, soft=true, binary=false, prio=0, kw...) + set_output_bounds!(mpc; ymin=_to_vec_bound(lb, mpc.model.ny), + ymax=_to_vec_bound(ub, mpc.model.ny), + ks=ks, soft=soft, binary=binary, prio=prio) +end + +# State: lb ≤ x ≤ ub → add_constraint! with Ax = I +function _add_constraint!(mpc::MPC, lb, ::StateRef, ub; + ks=2:mpc.Np, soft=false, binary=false, prio=0, kw...) + add_constraint!(mpc; + Ax = Matrix{Float64}(I, mpc.model.nx, mpc.model.nx), + lb = _to_vec_bound(lb, mpc.model.nx), + ub = _to_vec_bound(ub, mpc.model.nx), + ks=ks, soft=soft, binary=binary, prio=prio) +end + +# Disturbance: lb ≤ d ≤ ub → set_disturbance! +function _add_constraint!(mpc::MPC, lb, ::DisturbanceRef, ub; kw...) + set_disturbance!(mpc, _bound(lb), _bound(ub)) +end + +# General SignalExpr: lb ≤ Au*u + Ax*x + … ≤ ub → add_constraint! +function _add_constraint!(mpc::MPC, lb, expr::SignalExpr, ub; + ks=2:mpc.Np, soft=false, binary=false, prio=0, kw...) + Au = zeros(0,0); Ax = zeros(0,0) + Ar = zeros(0,0); Ad = zeros(0,0); Aup = zeros(0,0) + for term in expr.terms + A = term.A + if term.kind == :u; Au = isempty(Au) ? A : Au + A + elseif term.kind == :x; Ax = isempty(Ax) ? A : Ax + A + elseif term.kind == :r; Ar = isempty(Ar) ? A : Ar + A + elseif term.kind == :d; Ad = isempty(Ad) ? A : Ad + A + elseif term.kind == :uprev; Aup = isempty(Aup) ? A : Aup + A + end + end + add_constraint!(mpc; + Au = isempty(Au) ? nothing : Au, + Ax = isempty(Ax) ? nothing : Ax, + Ar = Ar, Ad = Ad, Aup = Aup, + lb = _bound(lb), ub = _bound(ub), + ks=ks, soft=soft, binary=binary, prio=prio) +end + +# One-sided: dispatch based on which side carries the signal +function _add_constraint_onesided!(mpc::MPC, lhs, op::Symbol, rhs; kw...) + if op == :(<=) + if _is_signal(lhs) + # signal <= bound → upper bound + _add_constraint!(mpc, nothing, lhs, rhs; kw...) + elseif _is_signal(rhs) + # bound <= signal → lower bound + _add_constraint!(mpc, lhs, rhs, nothing; kw...) + else + error("@constraint: no signal reference found in expression") + end + elseif op == :(>=) + if _is_signal(lhs) + # signal >= bound → lower bound + _add_constraint!(mpc, rhs, lhs, nothing; kw...) + elseif _is_signal(rhs) + # bound >= signal → upper bound + _add_constraint!(mpc, nothing, rhs, lhs; kw...) + else + error("@constraint: no signal reference found in expression") + end + else + error("@constraint: unsupported operator $op") + end +end + +# ---- Macro helper (parse keyword args from raw macro arg list) ------------- + +function _parse_macro_kwargs(args) + kw_pairs = Expr[] + for arg in args + if arg isa Expr && arg.head == :(=) + push!(kw_pairs, Expr(:kw, arg.args[1], esc(arg.args[2]))) + end + end + return kw_pairs +end + +# Build a function-call Expr with optional keyword arguments +function _make_call(fn, kw_pairs, pos_args...) + escaped = [esc(a) for a in pos_args] + if isempty(kw_pairs) + return Expr(:call, fn, escaped...) + else + return Expr(:call, fn, Expr(:parameters, kw_pairs...), escaped...) + end +end + +# ---- Public macros --------------------------------------------------------- + +""" + @control(mpc, name) + @control(mpc, name, umin=umin_val, umax=umax_val) + +Declare a control input signal reference named `name` for the MPC controller +`mpc`. The variable `name` is assigned a [`ControlRef`](@ref) that can be +used in [`@constraint`](@ref) expressions. + +Optionally set the input bounds `umin ≤ u ≤ umax` (equivalent to calling +[`set_input_bounds!`](@ref)). + +# Examples +```julia +@control mpc u +@control mpc u umin=-ones(2) umax=ones(2) +@constraint mpc -1 <= u <= 1 +``` +""" +macro control(mpc_ex, name_ex, args...) + umin_ex = nothing; umax_ex = nothing + for arg in args + (arg isa Expr && arg.head == :(=)) || continue + k = Symbol(arg.args[1]) + k == :umin && (umin_ex = arg.args[2]) + k == :umax && (umax_ex = arg.args[2]) + end + + result = Expr(:block) + push!(result.args, :($(esc(name_ex)) = LinearMPC.ControlRef($(esc(mpc_ex))))) + if !isnothing(umin_ex) || !isnothing(umax_ex) + um = isnothing(umin_ex) ? :(zeros(0)) : esc(umin_ex) + uM = isnothing(umax_ex) ? :(zeros(0)) : esc(umax_ex) + push!(result.args, + :(LinearMPC.set_input_bounds!($(esc(mpc_ex)); umin=$um, umax=$uM))) + end + push!(result.args, :($(esc(name_ex)))) + return result +end + +""" + @state(mpc, name) + +Declare a state signal reference named `name` for the MPC controller `mpc`. +The variable `name` is assigned a [`StateRef`](@ref) that can be used in +[`@constraint`](@ref) expressions. + +# Example +```julia +@state mpc x +@constraint mpc -5 <= x <= 5 +``` +""" +macro state(mpc_ex, name_ex) + quote + $(esc(name_ex)) = LinearMPC.StateRef($(esc(mpc_ex))) + end +end + +""" + @output(mpc, name) + +Declare an output signal reference named `name` for the MPC controller `mpc`. +The variable `name` is assigned an [`OutputRef`](@ref) that can be used in +[`@constraint`](@ref) expressions. + +Output constraints are soft by default (consistent with [`set_output_bounds!`](@ref)). + +# Example +```julia +@output mpc y +@constraint mpc 0 <= y <= 5 +``` +""" +macro output(mpc_ex, name_ex) + quote + $(esc(name_ex)) = LinearMPC.OutputRef($(esc(mpc_ex))) + end +end + +""" + @disturbance(mpc, name) + @disturbance(mpc, name, wmin=wmin_val, wmax=wmax_val) + +Declare a disturbance signal reference named `name` for the MPC controller +`mpc`. The variable `name` is assigned a [`DisturbanceRef`](@ref) that can +be used in [`@constraint`](@ref) expressions. + +Optionally set disturbance bounds (equivalent to calling +[`set_disturbance!`](@ref)). + +# Example +```julia +@disturbance mpc d wmin=-0.1*ones(2) wmax=0.1*ones(2) +``` +""" +macro disturbance(mpc_ex, name_ex, args...) + wmin_ex = nothing; wmax_ex = nothing + for arg in args + (arg isa Expr && arg.head == :(=)) || continue + k = Symbol(arg.args[1]) + k == :wmin && (wmin_ex = arg.args[2]) + k == :wmax && (wmax_ex = arg.args[2]) + end + + result = Expr(:block) + push!(result.args, :($(esc(name_ex)) = LinearMPC.DisturbanceRef($(esc(mpc_ex))))) + if !isnothing(wmin_ex) || !isnothing(wmax_ex) + wm = isnothing(wmin_ex) ? :(zeros(0)) : esc(wmin_ex) + wM = isnothing(wmax_ex) ? :(zeros(0)) : esc(wmax_ex) + push!(result.args, + :(LinearMPC.set_disturbance!($(esc(mpc_ex)), $wm, $wM))) + end + push!(result.args, :($(esc(name_ex)))) + return result +end + +""" + @constraint(mpc, lb <= signal <= ub) + @constraint(mpc, signal <= ub) + @constraint(mpc, signal >= lb) + @constraint(mpc, lb <= A*signal1 + B*signal2 <= ub) + @constraint(mpc, expr, soft=true, ks=2:5, binary=false, prio=0) + +Add a constraint to the MPC controller `mpc`. + +The `signal` must be a reference created by [`@control`](@ref), +[`@state`](@ref), [`@output`](@ref), or [`@disturbance`](@ref). + +| Signal type | Calls | Default `soft` | +|:-------------|:---------------------------|:---------------| +| `ControlRef` | `set_input_bounds!` | N/A | +| `OutputRef` | `set_output_bounds!` | `true` | +| `StateRef` | `add_constraint!` (Ax = I) | `false` | +| `SignalExpr` | `add_constraint!` | `false` | + +Optional keyword arguments (for `StateRef` and `SignalExpr` constraints): +- `soft=false`: Penalise violations instead of enforcing them hard +- `binary=false`: Enforce with equality +- `prio=0`: Priority level for hierarchical optimisation +- `ks=2:mpc.Np`: Time steps at which the constraint is active + +# Examples +```julia +@control mpc u +@output mpc y +@state mpc x + +@constraint mpc -1 <= u <= 1 # input bounds (hard) +@constraint mpc 0 <= y <= 5 # output bounds (soft by default) +@constraint mpc u <= 0.5 # one-sided upper bound +@constraint mpc -1 <= u <= 1 soft=true # explicit soft constraint +@constraint mpc lb <= Au*u + Ax*x <= ub # general linear +@constraint mpc lb <= Au*u + Ax*x <= ub soft=true ks=3:8 +``` +""" +macro constraint(mpc_ex, expr, args...) + kw_pairs = _parse_macro_kwargs(args) + + # Double-sided: lb <= mid <= ub or lb >= mid >= ub + if expr isa Expr && expr.head == :comparison && length(expr.args) == 5 + lb_ex, op1, mid_ex, op2, ub_ex = expr.args + if op1 == :(<=) && op2 == :(<=) + return _make_call(:(LinearMPC._add_constraint!), kw_pairs, + mpc_ex, lb_ex, mid_ex, ub_ex) + elseif op1 == :(>=) && op2 == :(>=) + # a >= mid >= b ≡ b <= mid <= a + return _make_call(:(LinearMPC._add_constraint!), kw_pairs, + mpc_ex, ub_ex, mid_ex, lb_ex) + end + end + + # Single-sided: a <= b or a >= b + if expr isa Expr && expr.head == :call && length(expr.args) == 3 + op, lhs_ex, rhs_ex = expr.args + if op in (:(<=), :(>=)) + return _make_call(:(LinearMPC._add_constraint_onesided!), kw_pairs, + mpc_ex, lhs_ex, QuoteNode(op), rhs_ex) + end + end + + return :(error("@constraint: unsupported expression `" * string($(QuoteNode(expr))) * "`\n" * + "Expected: lb <= signal <= ub, signal <= ub, or signal >= lb")) +end + +""" + @objective(mpc, Q=Q_val, R=R_val, Rr=Rr_val, S=S_val, Qf=Qf_val, Qfx=Qfx_val) + +Set the objective function weights for the MPC controller. +Equivalent to [`set_objective!`](@ref)`(mpc; Q=Q_val, R=R_val, …)`. + +A vector is interpreted as a diagonal weight matrix. + +# Example +```julia +using LinearAlgebra +@objective mpc Q=I R=0.1*I Rr=0.01*I +``` +""" +macro objective(mpc_ex, args...) + kw_pairs = _parse_macro_kwargs(args) + return _make_call(:(LinearMPC.set_objective!), kw_pairs, mpc_ex) +end From efbe1224c667ddf7a6a2457ca3569b8bf6a0a922 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 19 Mar 2026 11:09:11 +0000 Subject: [PATCH 3/5] Add @dynamics macro and MPC(nx,nu) constructor for macro-based setup Co-authored-by: darnstrom <55484604+darnstrom@users.noreply.github.com> --- src/LinearMPC.jl | 4 +- src/macros.jl | 184 ++++++++++++++++++++++++++++++++++++++++++++--- src/types.jl | 25 +++++++ 3 files changed, 202 insertions(+), 11 deletions(-) diff --git a/src/LinearMPC.jl b/src/LinearMPC.jl index d2a60a4f..ef501f79 100644 --- a/src/LinearMPC.jl +++ b/src/LinearMPC.jl @@ -51,8 +51,8 @@ export predict_state!,correct_state! export set_state!,get_state,update_state! include("macros.jl"); -export ControlRef, StateRef, OutputRef, DisturbanceRef, SignalExpr -export @control, @state, @output, @disturbance, @constraint, @objective +export ControlRef, StateRef, OutputRef, DisturbanceRef, SignalExpr, DynamicsExpr +export @control, @state, @output, @disturbance, @constraint, @objective, @dynamics using PrecompileTools @setup_workload begin diff --git a/src/macros.jl b/src/macros.jl index 8e1e0d69..cc0b745c 100644 --- a/src/macros.jl +++ b/src/macros.jl @@ -4,11 +4,15 @@ # # Usage example: # -# mpc = MPC(F, G; C=C, Np=10) -# @control mpc u umin=-ones(2) umax=ones(2) -# @state mpc x -# @output mpc y -# @disturbance mpc d wmin=-0.1 wmax=0.1 +# mpc = MPC(2, 1; Np=10) # create with just state/control dimensions +# x = @state mpc x # declare signal references +# u = @control mpc u umin=-ones(1) umax=ones(1) +# d = @disturbance mpc d +# +# A = [1.0 0.1; 0.0 1.0]; B = [0.0; 1.0] +# @dynamics mpc x_next = A*x + B*u # set dynamics +# +# y = @output mpc y # @objective mpc Q=I R=0.1*I Rr=0.01*I # @constraint mpc -1 <= u <= 1 # @constraint mpc 0 <= y <= 5 @@ -88,26 +92,32 @@ end """ A * u where u::ControlRef -Produce a weighted control signal expression for use in `@constraint`. +Produce a weighted control signal expression for use in `@constraint` and `@dynamics`. """ Base.:*(A::AbstractMatrix, u::ControlRef) = SignalExpr(u.mpc, [SignalTerm(float(A), :u)]) +Base.:*(A::AbstractVector, u::ControlRef) = + SignalExpr(u.mpc, [SignalTerm(reshape(float(A), :, 1), :u)]) """ A * x where x::StateRef -Produce a weighted state signal expression for use in `@constraint`. +Produce a weighted state signal expression for use in `@constraint` and `@dynamics`. """ Base.:*(A::AbstractMatrix, x::StateRef) = SignalExpr(x.mpc, [SignalTerm(float(A), :x)]) +Base.:*(A::AbstractVector, x::StateRef) = + SignalExpr(x.mpc, [SignalTerm(reshape(float(A), :, 1), :x)]) """ A * d where d::DisturbanceRef -Produce a weighted disturbance signal expression for use in `@constraint`. +Produce a weighted disturbance signal expression for use in `@constraint` and `@dynamics`. """ Base.:*(A::AbstractMatrix, d::DisturbanceRef) = SignalExpr(d.mpc, [SignalTerm(float(A), :d)]) +Base.:*(A::AbstractVector, d::DisturbanceRef) = + SignalExpr(d.mpc, [SignalTerm(reshape(float(A), :, 1), :d)]) """Combine two `SignalExpr` values (sum).""" function Base.:+(e1::SignalExpr, e2::SignalExpr) @@ -115,6 +125,99 @@ function Base.:+(e1::SignalExpr, e2::SignalExpr) SignalExpr(e1.mpc, [e1.terms; e2.terms]) end +# ---- DynamicsExpr: SignalExpr + optional constant offset ------------------- + +""" + DynamicsExpr(signals, offset) + +Internal type representing the right-hand side of a dynamics equation: +`x_{k+1} = F*x_k + G*u_k + Gd*d_k + f_offset` + +Built automatically by expressions like `F*x + G*u + f_offset` where `x`, +`u` are signal references. Passed to [`_set_dynamics!`](@ref) by the +[`@dynamics`](@ref) macro. +""" +struct DynamicsExpr + signals::SignalExpr + offset::Vector{Float64} +end + +DynamicsExpr(expr::SignalExpr) = DynamicsExpr(expr, zeros(0)) + +Base.:+(e::SignalExpr, offset::AbstractVector) = DynamicsExpr(e, float(offset)) +Base.:+(offset::AbstractVector, e::SignalExpr) = DynamicsExpr(e, float(offset)) +Base.:+(e::DynamicsExpr, offset::AbstractVector) = DynamicsExpr(e.signals, e.offset + offset) +Base.:+(offset::AbstractVector, e::DynamicsExpr) = DynamicsExpr(e.signals, e.offset + offset) +Base.:+(e1::DynamicsExpr, e2::SignalExpr) = DynamicsExpr(e1.signals + e2, e1.offset) +Base.:+(e1::SignalExpr, e2::DynamicsExpr) = DynamicsExpr(e1 + e2.signals, e2.offset) + +# Scalar multiplication: allow e.g. `0.5*x` (treats signal as identity-weighted) +Base.:*(a::Number, u::ControlRef) = + SignalExpr(u.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u)]) +Base.:*(a::Number, x::StateRef) = + SignalExpr(x.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x)]) +Base.:*(a::Number, d::DisturbanceRef) = + SignalExpr(d.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d)]) + +# Bare signal with no matrix weight → identity +_bare(u::ControlRef) = SignalExpr(u.mpc, [SignalTerm(Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u)]) +_bare(x::StateRef) = SignalExpr(x.mpc, [SignalTerm(Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x)]) +_bare(d::DisturbanceRef) = SignalExpr(d.mpc, [SignalTerm(Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d)]) + +# Allow bare signal + something: x + G*u, etc. +Base.:+(x::StateRef, e::SignalExpr) = _bare(x) + e +Base.:+(e::SignalExpr, x::StateRef) = e + _bare(x) +Base.:+(u::ControlRef, e::SignalExpr) = _bare(u) + e +Base.:+(e::SignalExpr, u::ControlRef) = e + _bare(u) +Base.:+(d::DisturbanceRef, e::SignalExpr) = _bare(d) + e +Base.:+(e::SignalExpr, d::DisturbanceRef) = e + _bare(d) + +# ---- _set_dynamics!: update mpc.model from a DynamicsExpr ----------------- + +""" + _set_dynamics!(mpc, expr) + +Internal function used by [`@dynamics`](@ref). Extracts F, G, Gd, and +f_offset from `expr` (a `SignalExpr` or `DynamicsExpr`) and rebuilds +`mpc.model` accordingly, preserving the output matrix C, offset h_offset, +and sample time Ts. +""" +function _set_dynamics!(mpc::MPC, expr::SignalExpr) + _set_dynamics!(mpc, DynamicsExpr(expr)) +end + +function _set_dynamics!(mpc::MPC, dexpr::DynamicsExpr) + model = mpc.model + nx, nu, nd = model.nx, model.nu, model.nd + + F = zeros(nx, nx) + G = zeros(nx, nu) + Gd = zeros(nx, nd) + f_offset = isempty(dexpr.offset) ? zeros(nx) : Vector{Float64}(dexpr.offset) + + for term in dexpr.signals.terms + A = term.A + if term.kind == :x; F .+= A + elseif term.kind == :u; G .+= A + elseif term.kind == :d; Gd .+= A + end + end + + mpc.model = Model(F, G; + Gd = Gd, + C = model.C, + Dd = model.Dd, + f_offset = f_offset, + h_offset = model.h_offset, + Ts = model.Ts, + xo = model.xo, + uo = model.uo, + wmin = model.wmin, + wmax = model.wmax) + mpc.mpqp_issetup = false + return mpc +end + # ---- Internal helpers ------------------------------------------------------ # Convert `nothing` → empty Float64 vector (for optional bound arguments) @@ -423,8 +526,71 @@ macro constraint(mpc_ex, expr, args...) end """ - @objective(mpc, Q=Q_val, R=R_val, Rr=Rr_val, S=S_val, Qf=Qf_val, Qfx=Qfx_val) + @dynamics(mpc, x_next = F*x + G*u) + @dynamics(mpc, x_next = F*x + G*u + Gd*d) + @dynamics(mpc, x_next = F*x + G*u + f_offset) + @dynamics(mpc, x_next = F*x + G*u + Gd*d + f_offset) + +Set (or update) the system dynamics on the MPC controller `mpc`. + +The right-hand side must be an expression built from signal references +(`x::StateRef`, `u::ControlRef`, `d::DisturbanceRef`) created by the +[`@state`](@ref), [`@control`](@ref), and [`@disturbance`](@ref) macros, plus +optional constant vectors for the affine offset. + +The state, control, and disturbance matrices are extracted automatically from +the expression: +| Expression term | Interpretation | +|:----------------|:--------------------| +| `F*x` | State matrix F | +| `G*u` | Input matrix G | +| `Gd*d` | Disturbance matrix Gd| +| `f_offset` | Affine offset vector | + +The left-hand side name is ignored (any symbol may be used). +The C / Dd / h_offset / Ts / operating-point settings on the existing model +are preserved. + +This macro is particularly useful when combined with [`MPC(nx, nu)`](@ref), +which creates an MPC controller from state and control dimensions alone +without requiring the matrices upfront. + +# Examples +```julia +# Basic usage with an existing MPC +mpc = LinearMPC.MPC(F, G; Np=10) +@state mpc x; @control mpc u +@dynamics mpc x_next = F_new*x + G_new*u # update dynamics in place + +# Build an MPC purely through macros (no matrices needed upfront) +mpc = LinearMPC.MPC(2, 1; Np=10) # 2 states, 1 control +@state mpc x; @control mpc u + +A = [1.0 0.1; 0.0 1.0]; B = [0.0; 1.0] +@dynamics mpc x_next = A*x + B*u + +@objective mpc Q=I R=0.1 +@constraint mpc -1 <= u <= 1 +setup!(mpc) + +# With disturbance and affine offset +mpc = LinearMPC.MPC(2, 1; nd=1, Np=10) +@state mpc x; @control mpc u; @disturbance mpc d +Gd = [0.1; 0.0] +fo = [0.01; 0.0] +@dynamics mpc x_next = A*x + B*u + Gd*d + fo +``` +""" +macro dynamics(mpc_ex, eq_ex) + (eq_ex isa Expr && eq_ex.head == :(=)) || + error("@dynamics: expected an assignment expression, e.g. `x_next = F*x + G*u`") + rhs = eq_ex.args[2] + return :(LinearMPC._set_dynamics!($(esc(mpc_ex)), $(esc(rhs)))) +end + +""" + @objective(mpc, Q=Q_val, R=R_val, Rr=Rr_val, S=S_val, Qf=Qf_val, Qfx=Qfx_val) Set the objective function weights for the MPC controller. Equivalent to [`set_objective!`](@ref)`(mpc; Q=Q_val, R=R_val, …)`. diff --git a/src/types.jl b/src/types.jl index 4f33ff38..3de8d7c0 100644 --- a/src/types.jl +++ b/src/types.jl @@ -159,6 +159,31 @@ function MPC(F,G;Gd=zeros(0,0), C=zeros(0,0), Dd= zeros(0,0), f_offset=zeros(0), MPC(Model(F,G;Gd,f_offset,C,Dd,Ts);Np,Nc); end +""" + MPC(nx, nu; nd=0, ny=nx, Np=10, Nc=Np) + +Create an MPC controller with `nx` states and `nu` controls without +specifying system matrices upfront. The dynamics are initialised to zero +matrices and should be set afterwards with the [`@dynamics`](@ref) macro: + +```julia +mpc = LinearMPC.MPC(2, 1; Np=10) +@state mpc x; @control mpc u +A = [1.0 0.1; 0.0 1.0]; B = [0.0; 1.0] +@dynamics mpc x_next = A*x + B*u +``` + +Use `nd` to declare disturbance inputs and `ny` to set the output dimension +(defaults to `nx` when not provided, i.e. full-state output). +""" +function MPC(nx::Int, nu::Int; nd::Int=0, ny::Int=nx, Np=10, Nc=Np) + F = zeros(nx, nx) + G = zeros(nx, nu) + Gd = zeros(nx, nd) + C = Matrix{Float64}(I, ny, nx) + MPC(Model(F, G; Gd=Gd, C=C); Np=Np, Nc=Nc) +end + function MPC(A,B,Ts::Float64; Bd = zeros(0,0), f_offset=zeros(0), C = zeros(0,0), Dd = zeros(0,0), Np=10, Nc=Np) MPC(Model(A,B,Ts;Bd,f_offset,C,Dd);Np,Nc) end From b1431b156a449dd60ab2bd438ea9afef41c76840 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 19 Mar 2026 13:24:41 +0000 Subject: [PATCH 4/5] Add row-by-row @dynamics (style 2) with incremental @state/@control, subtraction overloads, MPC(;Np) constructor Co-authored-by: darnstrom <55484604+darnstrom@users.noreply.github.com> --- src/macros.jl | 710 +++++++++++++++++++++++++++++++++----------------- 1 file changed, 472 insertions(+), 238 deletions(-) diff --git a/src/macros.jl b/src/macros.jl index cc0b745c..0b66b827 100644 --- a/src/macros.jl +++ b/src/macros.jl @@ -2,46 +2,77 @@ # JuMP-like macro interface for LinearMPC # ============================================================================ # -# Usage example: +# Two usage styles are supported: # -# mpc = MPC(2, 1; Np=10) # create with just state/control dimensions -# x = @state mpc x # declare signal references -# u = @control mpc u umin=-ones(1) umax=ones(1) -# d = @disturbance mpc d +# --- Style 1: full-matrix (known dimensions upfront) --- # -# A = [1.0 0.1; 0.0 1.0]; B = [0.0; 1.0] -# @dynamics mpc x_next = A*x + B*u # set dynamics -# -# y = @output mpc y -# @objective mpc Q=I R=0.1*I Rr=0.01*I +# mpc = MPC(F, G; C=C, Np=10) +# @control mpc u umin=-ones(2) umax=ones(2) +# @state mpc x +# @output mpc y +# @disturbance mpc d +# @dynamics mpc x_next = F_new*x + G_new*u # full F and G matrices +# @objective mpc Q=I R=0.1*I # @constraint mpc -1 <= u <= 1 # @constraint mpc 0 <= y <= 5 -# @constraint mpc lb <= Au*u + Ax*x <= ub # general linear constraint -# @constraint mpc u <= 0.5 -# @constraint mpc -1 <= u <= 1 soft=true # setup!(mpc) +# +# --- Style 2: incremental (dimensions built row by row) --- +# +# mpc = MPC(Np=10) # zero-dimensional MPC +# @state mpc x1 # x1 = StateRef(mpc,1), x1_next also defined +# @state mpc x2 # x2 = StateRef(mpc,2), x2_next also defined +# @control mpc u1 # u1 = ControlRef(mpc,1) +# @dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u1 # sets row 1 of F,G +# @dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # row 2; G[2,:] stays 0 +# @objective mpc Q=I R=0.1 +# @constraint mpc -1 <= u1 <= 1 +# setup!(mpc) + +# ---- Module-level registry for incremental-mode MPCs ---------------------- +# Maps each incremental MPC (by object identity) to its (nx_count, nu_count, nd_count). +const _incremental_registry = WeakKeyDict{MPC, NTuple{3,Int}}() + +""" +Return true if `mpc` was created in incremental mode (via `MPC(; Np=...)`). +""" +_is_incremental(mpc::MPC) = haskey(_incremental_registry, mpc) # ---- Signal reference types ------------------------------------------------ """ + ControlRef(mpc, idx) ControlRef(mpc) Reference to the control input signal of an MPC controller. -Created by the [`@control`](@ref) macro for use in [`@constraint`](@ref) expressions. + +- `idx = 0` -- refers to the full control vector (style 1). +- `idx > 0` -- refers to a single control variable at column `idx` (style 2). + +Created by the [`@control`](@ref) macro. """ struct ControlRef mpc::MPC + idx::Int end +ControlRef(mpc::MPC) = ControlRef(mpc, 0) """ + StateRef(mpc, idx) StateRef(mpc) Reference to the state signal of an MPC controller. -Created by the [`@state`](@ref) macro for use in [`@constraint`](@ref) expressions. + +- `idx = 0` -- refers to the full state vector (style 1). +- `idx > 0` -- refers to a single state variable at row/column `idx` (style 2). + +Created by the [`@state`](@ref) macro. """ struct StateRef mpc::MPC + idx::Int end +StateRef(mpc::MPC) = StateRef(mpc, 0) """ OutputRef(mpc) @@ -54,183 +85,242 @@ struct OutputRef end """ + DisturbanceRef(mpc, idx) DisturbanceRef(mpc) Reference to the disturbance signal of an MPC controller. -Created by the [`@disturbance`](@ref) macro for use in [`@constraint`](@ref) expressions. + +- `idx = 0` -- refers to the full disturbance vector (style 1). +- `idx > 0` -- refers to a single disturbance variable at column `idx` (style 2). + +Created by the [`@disturbance`](@ref) macro. """ struct DisturbanceRef mpc::MPC + idx::Int end +DisturbanceRef(mpc::MPC) = DisturbanceRef(mpc, 0) -# ---- SignalExpr: weighted combination of signals --------------------------- +# ---- SignalTerm and SignalExpr --------------------------------------------- -# Internal: A * [signal_kind] term +""" +Internal: A * [signal_kind] term, possibly at a specific column. + +- `col_idx == 0`: dense matrix coefficient (full vector signal). +- `col_idx > 0`: scalar coefficient at column `col_idx` (single signal variable). +""" struct SignalTerm A::Matrix{Float64} - kind::Symbol # :u, :x, :r, :d, :uprev + kind::Symbol # :u, :x, :r, :d, :uprev + col_idx::Int # 0 = dense full matrix; i > 0 = scalar at column i end +SignalTerm(A, kind) = SignalTerm(A, kind, 0) """ SignalExpr(mpc, terms) -A linear combination of weighted MPC signals (e.g. `Au*u + Ax*x`), -used inside [`@constraint`](@ref) expressions. - -Built automatically when multiplying a matrix by a signal reference: -```julia -Au*u + Ax*x # where u::ControlRef, x::StateRef -``` +A linear combination of weighted MPC signals (e.g. `Au*u + Ax*x`). +Built automatically when multiplying a matrix by a signal reference. """ struct SignalExpr mpc::MPC terms::Vector{SignalTerm} end -# ---- Arithmetic overloads -------------------------------------------------- - -""" - A * u where u::ControlRef - -Produce a weighted control signal expression for use in `@constraint` and `@dynamics`. -""" -Base.:*(A::AbstractMatrix, u::ControlRef) = - SignalExpr(u.mpc, [SignalTerm(float(A), :u)]) -Base.:*(A::AbstractVector, u::ControlRef) = - SignalExpr(u.mpc, [SignalTerm(reshape(float(A), :, 1), :u)]) - -""" - A * x where x::StateRef - -Produce a weighted state signal expression for use in `@constraint` and `@dynamics`. -""" -Base.:*(A::AbstractMatrix, x::StateRef) = - SignalExpr(x.mpc, [SignalTerm(float(A), :x)]) -Base.:*(A::AbstractVector, x::StateRef) = - SignalExpr(x.mpc, [SignalTerm(reshape(float(A), :, 1), :x)]) - -""" - A * d where d::DisturbanceRef - -Produce a weighted disturbance signal expression for use in `@constraint` and `@dynamics`. -""" -Base.:*(A::AbstractMatrix, d::DisturbanceRef) = - SignalExpr(d.mpc, [SignalTerm(float(A), :d)]) -Base.:*(A::AbstractVector, d::DisturbanceRef) = - SignalExpr(d.mpc, [SignalTerm(reshape(float(A), :, 1), :d)]) - -"""Combine two `SignalExpr` values (sum).""" -function Base.:+(e1::SignalExpr, e2::SignalExpr) - @assert e1.mpc === e2.mpc "Signal expressions must refer to the same MPC controller" - SignalExpr(e1.mpc, [e1.terms; e2.terms]) -end - # ---- DynamicsExpr: SignalExpr + optional constant offset ------------------- """ DynamicsExpr(signals, offset) -Internal type representing the right-hand side of a dynamics equation: -`x_{k+1} = F*x_k + G*u_k + Gd*d_k + f_offset` - -Built automatically by expressions like `F*x + G*u + f_offset` where `x`, -`u` are signal references. Passed to [`_set_dynamics!`](@ref) by the -[`@dynamics`](@ref) macro. +Right-hand side of a dynamics equation: F*x + G*u + Gd*d + offset. """ struct DynamicsExpr signals::SignalExpr offset::Vector{Float64} end - DynamicsExpr(expr::SignalExpr) = DynamicsExpr(expr, zeros(0)) -Base.:+(e::SignalExpr, offset::AbstractVector) = DynamicsExpr(e, float(offset)) -Base.:+(offset::AbstractVector, e::SignalExpr) = DynamicsExpr(e, float(offset)) -Base.:+(e::DynamicsExpr, offset::AbstractVector) = DynamicsExpr(e.signals, e.offset + offset) -Base.:+(offset::AbstractVector, e::DynamicsExpr) = DynamicsExpr(e.signals, e.offset + offset) -Base.:+(e1::DynamicsExpr, e2::SignalExpr) = DynamicsExpr(e1.signals + e2, e1.offset) -Base.:+(e1::SignalExpr, e2::DynamicsExpr) = DynamicsExpr(e1 + e2.signals, e2.offset) +# ---- Arithmetic overloads -------------------------------------------------- -# Scalar multiplication: allow e.g. `0.5*x` (treats signal as identity-weighted) -Base.:*(a::Number, u::ControlRef) = - SignalExpr(u.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u)]) -Base.:*(a::Number, x::StateRef) = - SignalExpr(x.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x)]) -Base.:*(a::Number, d::DisturbanceRef) = - SignalExpr(d.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d)]) +# Dense matrix * full-state/control/disturbance ref (style 1) +Base.:*(A::AbstractMatrix, x::StateRef) = + SignalExpr(x.mpc, [SignalTerm(float(A), :x, 0)]) +Base.:*(A::AbstractMatrix, u::ControlRef) = + SignalExpr(u.mpc, [SignalTerm(float(A), :u, 0)]) +Base.:*(A::AbstractMatrix, d::DisturbanceRef) = + SignalExpr(d.mpc, [SignalTerm(float(A), :d, 0)]) -# Bare signal with no matrix weight → identity -_bare(u::ControlRef) = SignalExpr(u.mpc, [SignalTerm(Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u)]) -_bare(x::StateRef) = SignalExpr(x.mpc, [SignalTerm(Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x)]) -_bare(d::DisturbanceRef) = SignalExpr(d.mpc, [SignalTerm(Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d)]) +# Column-vector * full-state/control/disturbance ref (style 1, row-vector coefficient) +Base.:*(A::AbstractVector, x::StateRef) = + SignalExpr(x.mpc, [SignalTerm(reshape(float(A), :, 1), :x, 0)]) +Base.:*(A::AbstractVector, u::ControlRef) = + SignalExpr(u.mpc, [SignalTerm(reshape(float(A), :, 1), :u, 0)]) +Base.:*(A::AbstractVector, d::DisturbanceRef) = + SignalExpr(d.mpc, [SignalTerm(reshape(float(A), :, 1), :d, 0)]) + +# Scalar * ref: for indexed refs (style 2) -> sparse column term; +# for full-vector refs (style 1) -> scaled identity +function Base.:*(a::Number, x::StateRef) + if x.idx == 0 + nx = x.mpc.model.nx + return SignalExpr(x.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, nx, nx), :x, 0)]) + else + return SignalExpr(x.mpc, [SignalTerm(fill(Float64(a), 1, 1), :x, x.idx)]) + end +end +function Base.:*(a::Number, u::ControlRef) + if u.idx == 0 + nu = u.mpc.model.nu + return SignalExpr(u.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, nu, nu), :u, 0)]) + else + return SignalExpr(u.mpc, [SignalTerm(fill(Float64(a), 1, 1), :u, u.idx)]) + end +end +function Base.:*(a::Number, d::DisturbanceRef) + if d.idx == 0 + nd = d.mpc.model.nd + return SignalExpr(d.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, nd, nd), :d, 0)]) + else + return SignalExpr(d.mpc, [SignalTerm(fill(Float64(a), 1, 1), :d, d.idx)]) + end +end + +# Bare signal as identity (for use in `x + G*u` etc.) +_bare(x::StateRef) = x.idx == 0 ? + SignalExpr(x.mpc, [SignalTerm(Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x, 0)]) : + SignalExpr(x.mpc, [SignalTerm(fill(1.0, 1, 1), :x, x.idx)]) +_bare(u::ControlRef) = u.idx == 0 ? + SignalExpr(u.mpc, [SignalTerm(Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u, 0)]) : + SignalExpr(u.mpc, [SignalTerm(fill(1.0, 1, 1), :u, u.idx)]) +_bare(d::DisturbanceRef) = d.idx == 0 ? + SignalExpr(d.mpc, [SignalTerm(Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d, 0)]) : + SignalExpr(d.mpc, [SignalTerm(fill(1.0, 1, 1), :d, d.idx)]) + +# Combine two SignalExprs +function Base.:+(e1::SignalExpr, e2::SignalExpr) + @assert e1.mpc === e2.mpc "Signal expressions must refer to the same MPC controller" + SignalExpr(e1.mpc, [e1.terms; e2.terms]) +end -# Allow bare signal + something: x + G*u, etc. +# Unary negation and subtraction for SignalExpr +Base.:-(e::SignalExpr) = + SignalExpr(e.mpc, [SignalTerm(-term.A, term.kind, term.col_idx) for term in e.terms]) +Base.:-(e1::SignalExpr, e2::SignalExpr) = e1 + (-e2) +Base.:-(e::SignalExpr, x::StateRef) = e + (-_bare(x)) +Base.:-(e::SignalExpr, u::ControlRef) = e + (-_bare(u)) +Base.:-(e::SignalExpr, d::DisturbanceRef) = e + (-_bare(d)) +Base.:-(x::StateRef, e::SignalExpr) = _bare(x) + (-e) +Base.:-(u::ControlRef, e::SignalExpr) = _bare(u) + (-e) + +# Allow bare signal + something: x + G*u, u + F*x, etc. Base.:+(x::StateRef, e::SignalExpr) = _bare(x) + e Base.:+(e::SignalExpr, x::StateRef) = e + _bare(x) Base.:+(u::ControlRef, e::SignalExpr) = _bare(u) + e Base.:+(e::SignalExpr, u::ControlRef) = e + _bare(u) Base.:+(d::DisturbanceRef, e::SignalExpr) = _bare(d) + e Base.:+(e::SignalExpr, d::DisturbanceRef) = e + _bare(d) +Base.:+(x::StateRef, u::ControlRef) = _bare(x) + _bare(u) +Base.:+(u::ControlRef, x::StateRef) = _bare(u) + _bare(x) +Base.:+(x::StateRef, d::DisturbanceRef) = _bare(x) + _bare(d) +Base.:+(d::DisturbanceRef, x::StateRef) = _bare(d) + _bare(x) -# ---- _set_dynamics!: update mpc.model from a DynamicsExpr ----------------- +# DynamicsExpr: SignalExpr + constant offset vector +Base.:+(e::SignalExpr, offset::AbstractVector) = DynamicsExpr(e, float(offset)) +Base.:+(offset::AbstractVector, e::SignalExpr) = DynamicsExpr(e, float(offset)) +Base.:+(e::DynamicsExpr, offset::AbstractVector) = DynamicsExpr(e.signals, e.offset + offset) +Base.:+(offset::AbstractVector, e::DynamicsExpr) = DynamicsExpr(e.signals, e.offset + offset) +Base.:+(e1::DynamicsExpr, e2::SignalExpr) = DynamicsExpr(e1.signals + e2, e1.offset) +Base.:+(e1::SignalExpr, e2::DynamicsExpr) = DynamicsExpr(e1 + e2.signals, e2.offset) -""" - _set_dynamics!(mpc, expr) +# ---- Model expansion helpers (incremental mode) --------------------------- + +""" +Expand mpc.model to add one more state dimension. Returns the new state index. +The new state has zero rows/columns in F, G, Gd and is unobserved in C by default. +""" +function _expand_state!(mpc::MPC) + m = mpc.model + nx, nu, nd = m.nx, m.nu, m.nd + new_nx = nx + 1 + F_new = [m.F zeros(nx, 1); + zeros(1, nx) zeros(1, 1)] + G_new = [m.G; zeros(1, nu)] + Gd_new = [m.Gd; zeros(1, nd)] + # Default to full-state output (C = I) in incremental mode + C_new = Matrix{Float64}(I, new_nx, new_nx) + mpc.model = Model(F_new, G_new; + Gd = Gd_new, + C = C_new, + Dd = m.Dd, + f_offset = [m.f_offset; 0.0], + h_offset = zeros(new_nx), # h_offset grows with ny + xo = [m.xo; 0.0], + uo = m.uo, + wmin = [m.wmin; 0.0], + wmax = [m.wmax; 0.0], + Ts = m.Ts) + mpc.mpqp_issetup = false + nx_s, nu_s, nd_s = _incremental_registry[mpc] + _incremental_registry[mpc] = (nx_s + 1, nu_s, nd_s) + return new_nx # new state index +end -Internal function used by [`@dynamics`](@ref). Extracts F, G, Gd, and -f_offset from `expr` (a `SignalExpr` or `DynamicsExpr`) and rebuilds -`mpc.model` accordingly, preserving the output matrix C, offset h_offset, -and sample time Ts. """ -function _set_dynamics!(mpc::MPC, expr::SignalExpr) - _set_dynamics!(mpc, DynamicsExpr(expr)) +Expand mpc.model to add one more control dimension. Returns the new control index. +""" +function _expand_control!(mpc::MPC) + m = mpc.model + G_new = [m.G zeros(m.nx, 1)] + mpc.model = Model(m.F, G_new; + Gd = m.Gd, + C = m.C, + Dd = m.Dd, + f_offset = m.f_offset, + h_offset = m.h_offset, + xo = m.xo, + uo = [m.uo; 0.0], + wmin = m.wmin, + wmax = m.wmax, + Ts = m.Ts) + mpc.mpqp_issetup = false + nx_s, nu_s, nd_s = _incremental_registry[mpc] + _incremental_registry[mpc] = (nx_s, nu_s + 1, nd_s) + return m.nu + 1 # new control index end -function _set_dynamics!(mpc::MPC, dexpr::DynamicsExpr) - model = mpc.model - nx, nu, nd = model.nx, model.nu, model.nd - - F = zeros(nx, nx) - G = zeros(nx, nu) - Gd = zeros(nx, nd) - f_offset = isempty(dexpr.offset) ? zeros(nx) : Vector{Float64}(dexpr.offset) - - for term in dexpr.signals.terms - A = term.A - if term.kind == :x; F .+= A - elseif term.kind == :u; G .+= A - elseif term.kind == :d; Gd .+= A - end - end - - mpc.model = Model(F, G; - Gd = Gd, - C = model.C, - Dd = model.Dd, - f_offset = f_offset, - h_offset = model.h_offset, - Ts = model.Ts, - xo = model.xo, - uo = model.uo, - wmin = model.wmin, - wmax = model.wmax) +""" +Expand mpc.model to add one more disturbance dimension. Returns the new disturbance index. +""" +function _expand_disturbance!(mpc::MPC) + m = mpc.model + Gd_new = [m.Gd zeros(m.nx, 1)] + Dd_new = [m.Dd zeros(m.ny, 1)] + mpc.model = Model(m.F, m.G; + Gd = Gd_new, + C = m.C, + Dd = Dd_new, + f_offset = m.f_offset, + h_offset = m.h_offset, + xo = m.xo, + uo = m.uo, + wmin = m.wmin, + wmax = m.wmax, + Ts = m.Ts) mpc.mpqp_issetup = false - return mpc + nx_s, nu_s, nd_s = _incremental_registry[mpc] + _incremental_registry[mpc] = (nx_s, nu_s, nd_s + 1) + return m.nd + 1 # new disturbance index end # ---- Internal helpers ------------------------------------------------------ -# Convert `nothing` → empty Float64 vector (for optional bound arguments) _bound(::Nothing) = zeros(0) _bound(x) = x -# Expand a bound to a vector of length `n` when a scalar is provided. -# Useful so that `@constraint mpc 0 <= y <= 5` works for any ny. _to_vec_bound(::Nothing, n) = zeros(0) _to_vec_bound(x::AbstractVector, n) = x _to_vec_bound(x::Number, n) = fill(Float64(x), n) -# Check whether a value is a signal reference (for one-sided constraint dispatch) _is_signal(::ControlRef) = true _is_signal(::OutputRef) = true _is_signal(::StateRef) = true @@ -240,12 +330,10 @@ _is_signal(::Any) = false # ---- Core constraint dispatch ---------------------------------------------- -# Control: lb ≤ u ≤ ub → set_input_bounds! function _add_constraint!(mpc::MPC, lb, ::ControlRef, ub; kw...) set_input_bounds!(mpc; umin=_bound(lb), umax=_bound(ub)) end -# Output: lb ≤ y ≤ ub → set_output_bounds! function _add_constraint!(mpc::MPC, lb, ::OutputRef, ub; ks=2:mpc.Np, soft=true, binary=false, prio=0, kw...) set_output_bounds!(mpc; ymin=_to_vec_bound(lb, mpc.model.ny), @@ -253,7 +341,6 @@ function _add_constraint!(mpc::MPC, lb, ::OutputRef, ub; ks=ks, soft=soft, binary=binary, prio=prio) end -# State: lb ≤ x ≤ ub → add_constraint! with Ax = I function _add_constraint!(mpc::MPC, lb, ::StateRef, ub; ks=2:mpc.Np, soft=false, binary=false, prio=0, kw...) add_constraint!(mpc; @@ -263,12 +350,10 @@ function _add_constraint!(mpc::MPC, lb, ::StateRef, ub; ks=ks, soft=soft, binary=binary, prio=prio) end -# Disturbance: lb ≤ d ≤ ub → set_disturbance! function _add_constraint!(mpc::MPC, lb, ::DisturbanceRef, ub; kw...) set_disturbance!(mpc, _bound(lb), _bound(ub)) end -# General SignalExpr: lb ≤ Au*u + Ax*x + … ≤ ub → add_constraint! function _add_constraint!(mpc::MPC, lb, expr::SignalExpr, ub; ks=2:mpc.Np, soft=false, binary=false, prio=0, kw...) Au = zeros(0,0); Ax = zeros(0,0) @@ -290,24 +375,19 @@ function _add_constraint!(mpc::MPC, lb, expr::SignalExpr, ub; ks=ks, soft=soft, binary=binary, prio=prio) end -# One-sided: dispatch based on which side carries the signal function _add_constraint_onesided!(mpc::MPC, lhs, op::Symbol, rhs; kw...) if op == :(<=) if _is_signal(lhs) - # signal <= bound → upper bound _add_constraint!(mpc, nothing, lhs, rhs; kw...) elseif _is_signal(rhs) - # bound <= signal → lower bound _add_constraint!(mpc, lhs, rhs, nothing; kw...) else error("@constraint: no signal reference found in expression") end elseif op == :(>=) if _is_signal(lhs) - # signal >= bound → lower bound _add_constraint!(mpc, rhs, lhs, nothing; kw...) elseif _is_signal(rhs) - # bound >= signal → upper bound _add_constraint!(mpc, nothing, rhs, lhs; kw...) else error("@constraint: no signal reference found in expression") @@ -317,7 +397,7 @@ function _add_constraint_onesided!(mpc::MPC, lhs, op::Symbol, rhs; kw...) end end -# ---- Macro helper (parse keyword args from raw macro arg list) ------------- +# ---- Macro helper ---------------------------------------------------------- function _parse_macro_kwargs(args) kw_pairs = Expr[] @@ -329,7 +409,6 @@ function _parse_macro_kwargs(args) return kw_pairs end -# Build a function-call Expr with optional keyword arguments function _make_call(fn, kw_pairs, pos_args...) escaped = [esc(a) for a in pos_args] if isempty(kw_pairs) @@ -339,24 +418,139 @@ function _make_call(fn, kw_pairs, pos_args...) end end +# ---- Dynamics: full-matrix and row-by-row ---------------------------------- + +""" + _set_dynamics!(mpc, expr) + +Set the full F, G, Gd, f_offset from a `SignalExpr` or `DynamicsExpr`. +""" +function _set_dynamics!(mpc::MPC, expr::SignalExpr) + _set_dynamics!(mpc, DynamicsExpr(expr)) +end + +function _set_dynamics!(mpc::MPC, dexpr::DynamicsExpr) + model = mpc.model + nx, nu, nd = model.nx, model.nu, model.nd + + F = zeros(nx, nx) + G = zeros(nx, nu) + Gd = zeros(nx, nd) + f_offset = isempty(dexpr.offset) ? zeros(nx) : Vector{Float64}(dexpr.offset) + + for term in dexpr.signals.terms + A = term.A + if term.kind == :x; F .+= A + elseif term.kind == :u; G .+= A + elseif term.kind == :d; Gd .+= A + end + end + + mpc.model = Model(F, G; + Gd = Gd, + C = model.C, + Dd = model.Dd, + f_offset = f_offset, + h_offset = model.h_offset, + Ts = model.Ts, + xo = model.xo, + uo = model.uo, + wmin = model.wmin, + wmax = model.wmax) + mpc.mpqp_issetup = false + return mpc +end + +""" + _set_dynamics_row!(mpc, row_ref, expr) + +Set a single row of F, G, and Gd. `row_ref` must be a `StateRef` with +`idx > 0` (created by `@state` in incremental mode). + +Each `SignalTerm` in `expr` contributes: +- `col_idx == 0` (dense A): if A has one row, that row is used; otherwise row `r` of A. +- `col_idx > 0` (sparse): places scalar `A[1,1]` at the given column. + +Rows not specified via this function remain zero. +""" +function _set_dynamics_row!(mpc::MPC, row_ref::StateRef, expr) + r = row_ref.idx + r > 0 || error("_set_dynamics_row!: expected an indexed StateRef (idx > 0), got idx=0") + m = mpc.model + + F_new = copy(m.F) + G_new = copy(m.G) + Gd_new = copy(m.Gd) + fo_new = copy(m.f_offset) + + signals = expr isa DynamicsExpr ? expr.signals : expr + if expr isa DynamicsExpr && !isempty(expr.offset) + ofs = expr.offset + fo_new[r] += length(ofs) >= r ? ofs[r] : ofs[end] + end + + for term in signals.terms + A, col = term.A, term.col_idx + if term.kind == :x + if col == 0 + row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] + F_new[r, :] .+= row_vec + else + F_new[r, col] += A[1, 1] + end + elseif term.kind == :u + if col == 0 + row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] + G_new[r, :] .+= row_vec + else + G_new[r, col] += A[1, 1] + end + elseif term.kind == :d + if col == 0 + row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] + Gd_new[r, :] .+= row_vec + else + Gd_new[r, col] += A[1, 1] + end + end + end + + mpc.model = Model(F_new, G_new; + Gd = Gd_new, + C = m.C, + Dd = m.Dd, + f_offset = fo_new, + h_offset = m.h_offset, + Ts = m.Ts, + xo = m.xo, + uo = m.uo, + wmin = m.wmin, + wmax = m.wmax) + mpc.mpqp_issetup = false + return mpc +end + # ---- Public macros --------------------------------------------------------- """ @control(mpc, name) @control(mpc, name, umin=umin_val, umax=umax_val) -Declare a control input signal reference named `name` for the MPC controller -`mpc`. The variable `name` is assigned a [`ControlRef`](@ref) that can be -used in [`@constraint`](@ref) expressions. +Declare a control input signal reference. -Optionally set the input bounds `umin ≤ u ≤ umax` (equivalent to calling -[`set_input_bounds!`](@ref)). +**Style 1** (`MPC(F, G; ...)`): assigns a full-vector `ControlRef` with no model change. + +**Style 2** (`MPC(Np=...)`): increments `nu` by 1 and assigns an indexed `ControlRef`. + +Optionally set input bounds via `umin`/`umax`. # Examples ```julia -@control mpc u -@control mpc u umin=-ones(2) umax=ones(2) -@constraint mpc -1 <= u <= 1 +# Style 1 +mpc = MPC(F, G; Np=10); @control mpc u umin=-1 umax=1 +# Style 2 +mpc = MPC(Np=10); @state mpc x1; @control mpc u +@dynamics mpc x1_next = 0.9*x1 + 0.5*u ``` """ macro control(mpc_ex, name_ex, args...) @@ -369,7 +563,14 @@ macro control(mpc_ex, name_ex, args...) end result = Expr(:block) - push!(result.args, :($(esc(name_ex)) = LinearMPC.ControlRef($(esc(mpc_ex))))) + push!(result.args, quote + if LinearMPC._is_incremental($(esc(mpc_ex))) + _idx = LinearMPC._expand_control!($(esc(mpc_ex))) + $(esc(name_ex)) = LinearMPC.ControlRef($(esc(mpc_ex)), _idx) + else + $(esc(name_ex)) = LinearMPC.ControlRef($(esc(mpc_ex))) + end + end) if !isnothing(umin_ex) || !isnothing(umax_ex) um = isnothing(umin_ex) ? :(zeros(0)) : esc(umin_ex) uM = isnothing(umax_ex) ? :(zeros(0)) : esc(umax_ex) @@ -383,30 +584,47 @@ end """ @state(mpc, name) -Declare a state signal reference named `name` for the MPC controller `mpc`. -The variable `name` is assigned a [`StateRef`](@ref) that can be used in -[`@constraint`](@ref) expressions. +Declare a state signal reference. -# Example +**Style 1** (`MPC(F, G; ...)`): assigns a full-vector `StateRef` with no model change. + +**Style 2** (`MPC(Np=...)`): increments `nx` by 1 and assigns an indexed `StateRef`. +Also defines `name_next` (e.g., `x1_next`) for use as the LHS of [`@dynamics`](@ref). +Unspecified rows of F, G remain zero. + +# Examples ```julia -@state mpc x -@constraint mpc -5 <= x <= 5 +# Style 1 +mpc = MPC(F, G; Np=10); @state mpc x; @constraint mpc -5 <= x <= 5 +# Style 2 +mpc = MPC(Np=10) +@state mpc x1 # defines x1 (StateRef index 1) and x1_next +@state mpc x2 # defines x2 (StateRef index 2) and x2_next +@control mpc u +@dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u +@dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # row 2 of G stays zero ``` """ macro state(mpc_ex, name_ex) + name_ex isa Symbol || error("@state: expected a symbol for the state name") + next_sym = Symbol(string(name_ex) * "_next") quote - $(esc(name_ex)) = LinearMPC.StateRef($(esc(mpc_ex))) + if LinearMPC._is_incremental($(esc(mpc_ex))) + _idx = LinearMPC._expand_state!($(esc(mpc_ex))) + $(esc(name_ex)) = LinearMPC.StateRef($(esc(mpc_ex)), _idx) + $(esc(next_sym)) = LinearMPC.StateRef($(esc(mpc_ex)), _idx) + else + # Style 1: no model change; define both name and name_next as full-state refs + $(esc(name_ex)) = LinearMPC.StateRef($(esc(mpc_ex))) + $(esc(next_sym)) = LinearMPC.StateRef($(esc(mpc_ex))) + end end end """ @output(mpc, name) -Declare an output signal reference named `name` for the MPC controller `mpc`. -The variable `name` is assigned an [`OutputRef`](@ref) that can be used in -[`@constraint`](@ref) expressions. - -Output constraints are soft by default (consistent with [`set_output_bounds!`](@ref)). +Declare an output signal reference for use in [`@constraint`](@ref) expressions. # Example ```julia @@ -424,19 +642,19 @@ end @disturbance(mpc, name) @disturbance(mpc, name, wmin=wmin_val, wmax=wmax_val) -Declare a disturbance signal reference named `name` for the MPC controller -`mpc`. The variable `name` is assigned a [`DisturbanceRef`](@ref) that can -be used in [`@constraint`](@ref) expressions. +Declare a disturbance signal reference. -Optionally set disturbance bounds (equivalent to calling -[`set_disturbance!`](@ref)). +**Style 2** (`MPC(Np=...)`): increments `nd` by 1 and assigns an indexed +`DisturbanceRef`. Also defines `name_next` for use in [`@dynamics`](@ref). # Example ```julia -@disturbance mpc d wmin=-0.1*ones(2) wmax=0.1*ones(2) +@disturbance mpc d wmin=-0.1*ones(1) wmax=0.1*ones(1) ``` """ macro disturbance(mpc_ex, name_ex, args...) + name_ex isa Symbol || error("@disturbance: expected a symbol for the disturbance name") + next_sym = Symbol(string(name_ex) * "_next") wmin_ex = nothing; wmax_ex = nothing for arg in args (arg isa Expr && arg.head == :(=)) || continue @@ -446,7 +664,15 @@ macro disturbance(mpc_ex, name_ex, args...) end result = Expr(:block) - push!(result.args, :($(esc(name_ex)) = LinearMPC.DisturbanceRef($(esc(mpc_ex))))) + push!(result.args, quote + if LinearMPC._is_incremental($(esc(mpc_ex))) + _idx = LinearMPC._expand_disturbance!($(esc(mpc_ex))) + $(esc(name_ex)) = LinearMPC.DisturbanceRef($(esc(mpc_ex)), _idx) + $(esc(next_sym)) = LinearMPC.DisturbanceRef($(esc(mpc_ex)), _idx) + else + $(esc(name_ex)) = LinearMPC.DisturbanceRef($(esc(mpc_ex))) + end + end) if !isnothing(wmin_ex) || !isnothing(wmax_ex) wm = isnothing(wmin_ex) ? :(zeros(0)) : esc(wmin_ex) wM = isnothing(wmax_ex) ? :(zeros(0)) : esc(wmax_ex) @@ -466,9 +692,6 @@ end Add a constraint to the MPC controller `mpc`. -The `signal` must be a reference created by [`@control`](@ref), -[`@state`](@ref), [`@output`](@ref), or [`@disturbance`](@ref). - | Signal type | Calls | Default `soft` | |:-------------|:---------------------------|:---------------| | `ControlRef` | `set_input_bounds!` | N/A | @@ -476,43 +699,29 @@ The `signal` must be a reference created by [`@control`](@ref), | `StateRef` | `add_constraint!` (Ax = I) | `false` | | `SignalExpr` | `add_constraint!` | `false` | -Optional keyword arguments (for `StateRef` and `SignalExpr` constraints): -- `soft=false`: Penalise violations instead of enforcing them hard -- `binary=false`: Enforce with equality -- `prio=0`: Priority level for hierarchical optimisation -- `ks=2:mpc.Np`: Time steps at which the constraint is active - # Examples ```julia -@control mpc u -@output mpc y -@state mpc x - -@constraint mpc -1 <= u <= 1 # input bounds (hard) -@constraint mpc 0 <= y <= 5 # output bounds (soft by default) -@constraint mpc u <= 0.5 # one-sided upper bound -@constraint mpc -1 <= u <= 1 soft=true # explicit soft constraint -@constraint mpc lb <= Au*u + Ax*x <= ub # general linear -@constraint mpc lb <= Au*u + Ax*x <= ub soft=true ks=3:8 +@constraint mpc -1 <= u <= 1 +@constraint mpc 0 <= y <= 5 +@constraint mpc u <= 0.5 +@constraint mpc lb <= Au*u + Ax*x <= ub +@constraint mpc -1 <= u <= 1 soft=true ks=3:8 ``` """ macro constraint(mpc_ex, expr, args...) kw_pairs = _parse_macro_kwargs(args) - # Double-sided: lb <= mid <= ub or lb >= mid >= ub if expr isa Expr && expr.head == :comparison && length(expr.args) == 5 lb_ex, op1, mid_ex, op2, ub_ex = expr.args if op1 == :(<=) && op2 == :(<=) return _make_call(:(LinearMPC._add_constraint!), kw_pairs, mpc_ex, lb_ex, mid_ex, ub_ex) elseif op1 == :(>=) && op2 == :(>=) - # a >= mid >= b ≡ b <= mid <= a return _make_call(:(LinearMPC._add_constraint!), kw_pairs, mpc_ex, ub_ex, mid_ex, lb_ex) end end - # Single-sided: a <= b or a >= b if expr isa Expr && expr.head == :call && length(expr.args) == 3 op, lhs_ex, rhs_ex = expr.args if op in (:(<=), :(>=)) @@ -527,74 +736,64 @@ end """ @dynamics(mpc, x_next = F*x + G*u) - @dynamics(mpc, x_next = F*x + G*u + Gd*d) - @dynamics(mpc, x_next = F*x + G*u + f_offset) @dynamics(mpc, x_next = F*x + G*u + Gd*d + f_offset) + @dynamics(mpc, x1_next = 0.9*x1 - 0.2*x2 + 0.5*u1) -Set (or update) the system dynamics on the MPC controller `mpc`. +Set (or update) system dynamics. -The right-hand side must be an expression built from signal references -(`x::StateRef`, `u::ControlRef`, `d::DisturbanceRef`) created by the -[`@state`](@ref), [`@control`](@ref), and [`@disturbance`](@ref) macros, plus -optional constant vectors for the affine offset. +**Style 1** (full-matrix): the LHS `x_next` is a `StateRef` with `idx == 0` +(created by `@state mpc x` on a dimensioned MPC). The RHS provides F, G, Gd, +and an optional constant offset vector all at once. -The state, control, and disturbance matrices are extracted automatically from -the expression: +**Style 2** (row-by-row): the LHS `x1_next` is an indexed `StateRef` (created +by `@state mpc x1` on an incremental `MPC(Np=...)`). The RHS expression +defines *one row* of F and G using scalar or row-vector coefficients. +Calling `@dynamics` for each state sets the corresponding row; rows not +explicitly set remain zero. -| Expression term | Interpretation | -|:----------------|:--------------------| -| `F*x` | State matrix F | -| `G*u` | Input matrix G | -| `Gd*d` | Disturbance matrix Gd| -| `f_offset` | Affine offset vector | - -The left-hand side name is ignored (any symbol may be used). -The C / Dd / h_offset / Ts / operating-point settings on the existing model -are preserved. - -This macro is particularly useful when combined with [`MPC(nx, nu)`](@ref), -which creates an MPC controller from state and control dimensions alone -without requiring the matrices upfront. +The C, Dd, h_offset, Ts, and operating-point settings of the existing model +are always preserved. # Examples ```julia -# Basic usage with an existing MPC -mpc = LinearMPC.MPC(F, G; Np=10) -@state mpc x; @control mpc u -@dynamics mpc x_next = F_new*x + G_new*u # update dynamics in place - -# Build an MPC purely through macros (no matrices needed upfront) -mpc = LinearMPC.MPC(2, 1; Np=10) # 2 states, 1 control +# Style 1 -- full matrices +mpc = MPC(2, 1; Np=10) @state mpc x; @control mpc u - A = [1.0 0.1; 0.0 1.0]; B = [0.0; 1.0] @dynamics mpc x_next = A*x + B*u -@objective mpc Q=I R=0.1 -@constraint mpc -1 <= u <= 1 -setup!(mpc) +# Style 2 -- row by row (incremental) +mpc = MPC(Np=10) +@state mpc x1; @state mpc x2; @control mpc u +@dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u # row 1 of F and G +@dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # row 2; G[2,:] stays 0 -# With disturbance and affine offset -mpc = LinearMPC.MPC(2, 1; nd=1, Np=10) -@state mpc x; @control mpc u; @disturbance mpc d -Gd = [0.1; 0.0] -fo = [0.01; 0.0] -@dynamics mpc x_next = A*x + B*u + Gd*d + fo +# With disturbance and affine offset (style 2) +@disturbance mpc d +@dynamics mpc x1_next = 0.9*x1 + 0.5*u + 0.1*d ``` """ macro dynamics(mpc_ex, eq_ex) (eq_ex isa Expr && eq_ex.head == :(=)) || - error("@dynamics: expected an assignment expression, e.g. `x_next = F*x + G*u`") + error("@dynamics: expected an assignment expression, e.g. `x1_next = 0.9*x1 + 0.5*u`") + lhs = eq_ex.args[1] rhs = eq_ex.args[2] - return :(LinearMPC._set_dynamics!($(esc(mpc_ex)), $(esc(rhs)))) + return quote + let _lhs_val = $(esc(lhs)), _rhs_val = $(esc(rhs)) + if _lhs_val isa LinearMPC.StateRef && _lhs_val.idx > 0 + LinearMPC._set_dynamics_row!($(esc(mpc_ex)), _lhs_val, _rhs_val) + else + LinearMPC._set_dynamics!($(esc(mpc_ex)), _rhs_val) + end + end + end end """ @objective(mpc, Q=Q_val, R=R_val, Rr=Rr_val, S=S_val, Qf=Qf_val, Qfx=Qfx_val) -Set the objective function weights for the MPC controller. -Equivalent to [`set_objective!`](@ref)`(mpc; Q=Q_val, R=R_val, …)`. -A vector is interpreted as a diagonal weight matrix. +Set the objective function weights for the MPC controller. +Equivalent to [`set_objective!`](@ref)`(mpc; Q=Q_val, R=R_val, ...)`. # Example ```julia @@ -606,3 +805,38 @@ macro objective(mpc_ex, args...) kw_pairs = _parse_macro_kwargs(args) return _make_call(:(LinearMPC.set_objective!), kw_pairs, mpc_ex) end + +# ---- MPC constructor for incremental mode ---------------------------------- + +""" + MPC(; Np=10, Nc=Np) + +Create a zero-dimensional MPC controller in **incremental mode**. + +States, controls, and disturbances are added one at a time using the +[`@state`](@ref), [`@control`](@ref), and [`@disturbance`](@ref) macros. +Each `@state mpc x_i` call increments the state dimension by 1 and defines +both `x_i` (for RHS use) and `x_i_next` (for the LHS of [`@dynamics`](@ref)). + +Dynamics are specified row-by-row via [`@dynamics`](@ref); undefined rows +default to zero. + +# Example +```julia +mpc = MPC(Np=10) +@state mpc x1; @state mpc x2 +@control mpc u + +@dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u +@dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # G[2,:] stays zero + +@objective mpc Q=I R=0.1 +@constraint mpc -1 <= u <= 1 +setup!(mpc) +``` +""" +function MPC(; Np::Int=10, Nc::Int=Np) + mpc = MPC(Model(zeros(0,0), zeros(0,0)); Np=Np, Nc=Nc) + _incremental_registry[mpc] = (0, 0, 0) + return mpc +end From 92214ef8a1603aa20c18afe43c3498e9f86fc356 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Thu, 19 Mar 2026 14:28:36 +0000 Subject: [PATCH 5/5] Add vector variable support, flexible dynamics LHS, and AbstractVector{StateRef} arithmetic Co-authored-by: darnstrom <55484604+darnstrom@users.noreply.github.com> --- src/LinearMPC.jl | 1 + src/macros.jl | 918 ++++++++++++++++++++++++++--------------------- 2 files changed, 502 insertions(+), 417 deletions(-) diff --git a/src/LinearMPC.jl b/src/LinearMPC.jl index ef501f79..319a329e 100644 --- a/src/LinearMPC.jl +++ b/src/LinearMPC.jl @@ -52,6 +52,7 @@ export set_state!,get_state,update_state! include("macros.jl"); export ControlRef, StateRef, OutputRef, DisturbanceRef, SignalExpr, DynamicsExpr +export ControlVec, StateVec, DisturbanceVec export @control, @state, @output, @disturbance, @constraint, @objective, @dynamics using PrecompileTools diff --git a/src/macros.jl b/src/macros.jl index 0b66b827..cfbe4663 100644 --- a/src/macros.jl +++ b/src/macros.jl @@ -19,18 +19,18 @@ # # --- Style 2: incremental (dimensions built row by row) --- # -# mpc = MPC(Np=10) # zero-dimensional MPC -# @state mpc x1 # x1 = StateRef(mpc,1), x1_next also defined -# @state mpc x2 # x2 = StateRef(mpc,2), x2_next also defined -# @control mpc u1 # u1 = ControlRef(mpc,1) -# @dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u1 # sets row 1 of F,G -# @dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # row 2; G[2,:] stays 0 +# mpc = MPC(Np=10) # zero-dimensional MPC +# @state mpc x1 # x1 = StateRef(mpc,1), x1_next also defined +# @state mpc v[1:4] # v = StateVec(mpc,[2,3,4,5]), v_next also defined +# @control mpc u[1:2] # u = ControlVec(mpc,[1,2]) +# @dynamics mpc x1 = 0.9*x1 + 0.5*v[1] # set row 1 (can omit _next) +# @dynamics mpc v[1] = 0.8*v[1] + 0.2*v[2] + u[1] # set row for v[1] +# @dynamics mpc [v[1]; v[2]] = A*v[1:2] + B*u # set multiple rows at once # @objective mpc Q=I R=0.1 -# @constraint mpc -1 <= u1 <= 1 +# @constraint mpc -1 <= u <= 1 # setup!(mpc) # ---- Module-level registry for incremental-mode MPCs ---------------------- -# Maps each incremental MPC (by object identity) to its (nx_count, nu_count, nd_count). const _incremental_registry = WeakKeyDict{MPC, NTuple{3,Int}}() """ @@ -44,12 +44,8 @@ _is_incremental(mpc::MPC) = haskey(_incremental_registry, mpc) ControlRef(mpc, idx) ControlRef(mpc) -Reference to the control input signal of an MPC controller. - -- `idx = 0` -- refers to the full control vector (style 1). -- `idx > 0` -- refers to a single control variable at column `idx` (style 2). - -Created by the [`@control`](@ref) macro. +Reference to one control input (`idx > 0`) or the full control vector (`idx == 0`). +Created by [`@control`](@ref). """ struct ControlRef mpc::MPC @@ -61,12 +57,8 @@ ControlRef(mpc::MPC) = ControlRef(mpc, 0) StateRef(mpc, idx) StateRef(mpc) -Reference to the state signal of an MPC controller. - -- `idx = 0` -- refers to the full state vector (style 1). -- `idx > 0` -- refers to a single state variable at row/column `idx` (style 2). - -Created by the [`@state`](@ref) macro. +Reference to one state (`idx > 0`) or the full state vector (`idx == 0`). +Created by [`@state`](@ref). """ struct StateRef mpc::MPC @@ -77,8 +69,7 @@ StateRef(mpc::MPC) = StateRef(mpc, 0) """ OutputRef(mpc) -Reference to the output signal of an MPC controller. -Created by the [`@output`](@ref) macro for use in [`@constraint`](@ref) expressions. +Reference to the output signal. Created by [`@output`](@ref). """ struct OutputRef mpc::MPC @@ -88,12 +79,8 @@ end DisturbanceRef(mpc, idx) DisturbanceRef(mpc) -Reference to the disturbance signal of an MPC controller. - -- `idx = 0` -- refers to the full disturbance vector (style 1). -- `idx > 0` -- refers to a single disturbance variable at column `idx` (style 2). - -Created by the [`@disturbance`](@ref) macro. +Reference to one disturbance (`idx > 0`) or the full disturbance vector (`idx == 0`). +Created by [`@disturbance`](@ref). """ struct DisturbanceRef mpc::MPC @@ -101,38 +88,90 @@ struct DisturbanceRef end DisturbanceRef(mpc::MPC) = DisturbanceRef(mpc, 0) +# ---- Vector signal types --------------------------------------------------- + +""" + StateVec(mpc, indices) + +A named vector of state variables covering the given indices. +Created by `@state mpc v[1:4]`. + +Supports scalar and range indexing: +- `v[3]` → `StateRef(mpc, indices[3])` +- `v[2:4]` → `StateVec(mpc, indices[2:4])` +""" +struct StateVec + mpc::MPC + indices::Vector{Int} +end +Base.length(sv::StateVec) = length(sv.indices) +Base.getindex(sv::StateVec, i::Int) = StateRef(sv.mpc, sv.indices[i]) +Base.getindex(sv::StateVec, r::AbstractRange) = StateVec(sv.mpc, sv.indices[r]) +Base.iterate(sv::StateVec, s=1) = s > length(sv) ? nothing : (sv[s], s+1) + +""" + ControlVec(mpc, indices) + +A named vector of control variables. Created by `@control mpc u[1:2]`. +""" +struct ControlVec + mpc::MPC + indices::Vector{Int} +end +Base.length(cv::ControlVec) = length(cv.indices) +Base.getindex(cv::ControlVec, i::Int) = ControlRef(cv.mpc, cv.indices[i]) +Base.getindex(cv::ControlVec, r::AbstractRange) = ControlVec(cv.mpc, cv.indices[r]) +Base.iterate(cv::ControlVec, s=1) = s > length(cv) ? nothing : (cv[s], s+1) + +""" + DisturbanceVec(mpc, indices) + +A named vector of disturbance variables. Created by `@disturbance mpc d[1:2]`. +""" +struct DisturbanceVec + mpc::MPC + indices::Vector{Int} +end +Base.length(dv::DisturbanceVec) = length(dv.indices) +Base.getindex(dv::DisturbanceVec, i::Int) = DisturbanceRef(dv.mpc, dv.indices[i]) +Base.getindex(dv::DisturbanceVec, r::AbstractRange) = DisturbanceVec(dv.mpc, dv.indices[r]) +Base.iterate(dv::DisturbanceVec, s=1) = s > length(dv) ? nothing : (dv[s], s+1) + # ---- SignalTerm and SignalExpr --------------------------------------------- """ -Internal: A * [signal_kind] term, possibly at a specific column. +Internal: A * [signal_kind] term. -- `col_idx == 0`: dense matrix coefficient (full vector signal). -- `col_idx > 0`: scalar coefficient at column `col_idx` (single signal variable). +`col_indices`: +- `Int[]` (empty) -- dense term spanning all columns. +- `[i]` (single) -- scalar at column i. +- `[i,j,.]` (multi) -- sparse block at those columns. """ struct SignalTerm A::Matrix{Float64} kind::Symbol # :u, :x, :r, :d, :uprev - col_idx::Int # 0 = dense full matrix; i > 0 = scalar at column i + col_indices::Vector{Int} end -SignalTerm(A, kind) = SignalTerm(A, kind, 0) +SignalTerm(A, kind) = SignalTerm(A, kind, Int[]) +SignalTerm(A, kind, col::Int) = SignalTerm(A, kind, col == 0 ? Int[] : [col]) """ SignalExpr(mpc, terms) -A linear combination of weighted MPC signals (e.g. `Au*u + Ax*x`). -Built automatically when multiplying a matrix by a signal reference. +A linear combination of weighted MPC signals. Built automatically by the +arithmetic overloads on signal references. """ struct SignalExpr mpc::MPC terms::Vector{SignalTerm} end -# ---- DynamicsExpr: SignalExpr + optional constant offset ------------------- +# ---- DynamicsExpr ---------------------------------------------------------- """ DynamicsExpr(signals, offset) -Right-hand side of a dynamics equation: F*x + G*u + Gd*d + offset. +RHS of a dynamics equation: weighted signals + optional constant offset. """ struct DynamicsExpr signals::SignalExpr @@ -142,190 +181,204 @@ DynamicsExpr(expr::SignalExpr) = DynamicsExpr(expr, zeros(0)) # ---- Arithmetic overloads -------------------------------------------------- -# Dense matrix * full-state/control/disturbance ref (style 1) -Base.:*(A::AbstractMatrix, x::StateRef) = - SignalExpr(x.mpc, [SignalTerm(float(A), :x, 0)]) -Base.:*(A::AbstractMatrix, u::ControlRef) = - SignalExpr(u.mpc, [SignalTerm(float(A), :u, 0)]) -Base.:*(A::AbstractMatrix, d::DisturbanceRef) = - SignalExpr(d.mpc, [SignalTerm(float(A), :d, 0)]) - -# Column-vector * full-state/control/disturbance ref (style 1, row-vector coefficient) -Base.:*(A::AbstractVector, x::StateRef) = - SignalExpr(x.mpc, [SignalTerm(reshape(float(A), :, 1), :x, 0)]) -Base.:*(A::AbstractVector, u::ControlRef) = - SignalExpr(u.mpc, [SignalTerm(reshape(float(A), :, 1), :u, 0)]) -Base.:*(A::AbstractVector, d::DisturbanceRef) = - SignalExpr(d.mpc, [SignalTerm(reshape(float(A), :, 1), :d, 0)]) - -# Scalar * ref: for indexed refs (style 2) -> sparse column term; -# for full-vector refs (style 1) -> scaled identity +# Dense matrix * full-vector ref (idx == 0) +Base.:*(A::AbstractMatrix, x::StateRef) = SignalExpr(x.mpc, [SignalTerm(float(A), :x)]) +Base.:*(A::AbstractMatrix, u::ControlRef) = SignalExpr(u.mpc, [SignalTerm(float(A), :u)]) +Base.:*(A::AbstractMatrix, d::DisturbanceRef) = SignalExpr(d.mpc, [SignalTerm(float(A), :d)]) + +# Dense matrix * StateVec/ControlVec/DisturbanceVec (sparse block at col_indices) +Base.:*(A::AbstractMatrix, sv::StateVec) = SignalExpr(sv.mpc, [SignalTerm(float(A), :x, sv.indices)]) +Base.:*(A::AbstractMatrix, cv::ControlVec) = SignalExpr(cv.mpc, [SignalTerm(float(A), :u, cv.indices)]) +Base.:*(A::AbstractMatrix, dv::DisturbanceVec) = SignalExpr(dv.mpc, [SignalTerm(float(A), :d, dv.indices)]) + +# Vector coefficient * full-vector ref +Base.:*(A::AbstractVector, x::StateRef) = SignalExpr(x.mpc, [SignalTerm(reshape(float(A),:,1), :x)]) +Base.:*(A::AbstractVector, u::ControlRef) = SignalExpr(u.mpc, [SignalTerm(reshape(float(A),:,1), :u)]) +Base.:*(A::AbstractVector, d::DisturbanceRef) = SignalExpr(d.mpc, [SignalTerm(reshape(float(A),:,1), :d)]) + +# Scalar * scalar ref (idx > 0) or full-vector ref (idx == 0) function Base.:*(a::Number, x::StateRef) - if x.idx == 0 - nx = x.mpc.model.nx - return SignalExpr(x.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, nx, nx), :x, 0)]) - else - return SignalExpr(x.mpc, [SignalTerm(fill(Float64(a), 1, 1), :x, x.idx)]) - end + x.idx == 0 ? + SignalExpr(x.mpc, [SignalTerm(float(a)*Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x)]) : + SignalExpr(x.mpc, [SignalTerm(fill(Float64(a), 1, 1), :x, x.idx)]) end function Base.:*(a::Number, u::ControlRef) - if u.idx == 0 - nu = u.mpc.model.nu - return SignalExpr(u.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, nu, nu), :u, 0)]) - else - return SignalExpr(u.mpc, [SignalTerm(fill(Float64(a), 1, 1), :u, u.idx)]) - end + u.idx == 0 ? + SignalExpr(u.mpc, [SignalTerm(float(a)*Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u)]) : + SignalExpr(u.mpc, [SignalTerm(fill(Float64(a), 1, 1), :u, u.idx)]) end function Base.:*(a::Number, d::DisturbanceRef) - if d.idx == 0 - nd = d.mpc.model.nd - return SignalExpr(d.mpc, [SignalTerm(float(a) * Matrix{Float64}(I, nd, nd), :d, 0)]) - else - return SignalExpr(d.mpc, [SignalTerm(fill(Float64(a), 1, 1), :d, d.idx)]) - end + d.idx == 0 ? + SignalExpr(d.mpc, [SignalTerm(float(a)*Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d)]) : + SignalExpr(d.mpc, [SignalTerm(fill(Float64(a), 1, 1), :d, d.idx)]) +end + +# Scalar * StateVec/ControlVec/DisturbanceVec: one sparse term per element +function Base.:*(a::Number, sv::StateVec) + SignalExpr(sv.mpc, [SignalTerm(fill(Float64(a), 1, 1), :x, [j]) for j in sv.indices]) +end +function Base.:*(a::Number, cv::ControlVec) + SignalExpr(cv.mpc, [SignalTerm(fill(Float64(a), 1, 1), :u, [j]) for j in cv.indices]) +end +function Base.:*(a::Number, dv::DisturbanceVec) + SignalExpr(dv.mpc, [SignalTerm(fill(Float64(a), 1, 1), :d, [j]) for j in dv.indices]) +end + +# Matrix * Vector{StateRef} / Vector{ControlRef} / Vector{DisturbanceRef} +# (e.g., A * [x1; x2] where [x1;x2] is a vcat of indexed refs) +function Base.:*(A::AbstractMatrix, refs::AbstractVector{<:StateRef}) + isempty(refs) && error("Empty StateRef vector") + mpc = refs[1].mpc + all(r -> r.mpc === mpc && r.idx > 0, refs) || + error("@dynamics: mixed MPC or unindexed StateRefs in vector expression") + SignalExpr(mpc, [SignalTerm(float(A), :x, [r.idx for r in refs])]) +end +function Base.:*(A::AbstractMatrix, refs::AbstractVector{<:ControlRef}) + isempty(refs) && error("Empty ControlRef vector") + mpc = refs[1].mpc + all(r -> r.mpc === mpc && r.idx > 0, refs) || + error("@dynamics: mixed MPC or unindexed ControlRefs in vector expression") + SignalExpr(mpc, [SignalTerm(float(A), :u, [r.idx for r in refs])]) +end +function Base.:*(a::Number, refs::AbstractVector{<:StateRef}) + isempty(refs) && error("Empty StateRef vector") + SignalExpr(refs[1].mpc, [SignalTerm(fill(Float64(a),1,1), :x, [r.idx for r in refs])]) +end +function Base.:*(a::Number, refs::AbstractVector{<:ControlRef}) + isempty(refs) && error("Empty ControlRef vector") + SignalExpr(refs[1].mpc, [SignalTerm(fill(Float64(a),1,1), :u, [r.idx for r in refs])]) end -# Bare signal as identity (for use in `x + G*u` etc.) +# Bare signal -> identity SignalExpr _bare(x::StateRef) = x.idx == 0 ? - SignalExpr(x.mpc, [SignalTerm(Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x, 0)]) : + SignalExpr(x.mpc, [SignalTerm(Matrix{Float64}(I, x.mpc.model.nx, x.mpc.model.nx), :x)]) : SignalExpr(x.mpc, [SignalTerm(fill(1.0, 1, 1), :x, x.idx)]) _bare(u::ControlRef) = u.idx == 0 ? - SignalExpr(u.mpc, [SignalTerm(Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u, 0)]) : + SignalExpr(u.mpc, [SignalTerm(Matrix{Float64}(I, u.mpc.model.nu, u.mpc.model.nu), :u)]) : SignalExpr(u.mpc, [SignalTerm(fill(1.0, 1, 1), :u, u.idx)]) _bare(d::DisturbanceRef) = d.idx == 0 ? - SignalExpr(d.mpc, [SignalTerm(Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d, 0)]) : + SignalExpr(d.mpc, [SignalTerm(Matrix{Float64}(I, d.mpc.model.nd, d.mpc.model.nd), :d)]) : SignalExpr(d.mpc, [SignalTerm(fill(1.0, 1, 1), :d, d.idx)]) +_bare(sv::StateVec) = SignalExpr(sv.mpc, [SignalTerm(fill(1.0,1,1), :x, [j]) for j in sv.indices]) +_bare(cv::ControlVec) = SignalExpr(cv.mpc, [SignalTerm(fill(1.0,1,1), :u, [j]) for j in cv.indices]) +_bare(dv::DisturbanceVec) = SignalExpr(dv.mpc, [SignalTerm(fill(1.0,1,1), :d, [j]) for j in dv.indices]) +function _bare(refs::AbstractVector{<:StateRef}) + isempty(refs) && error("Empty StateRef vector") + SignalExpr(refs[1].mpc, [SignalTerm(fill(1.0,1,1), :x, [r.idx for r in refs])]) +end +function _bare(refs::AbstractVector{<:ControlRef}) + isempty(refs) && error("Empty ControlRef vector") + SignalExpr(refs[1].mpc, [SignalTerm(fill(1.0,1,1), :u, [r.idx for r in refs])]) +end -# Combine two SignalExprs +# Combine SignalExprs function Base.:+(e1::SignalExpr, e2::SignalExpr) - @assert e1.mpc === e2.mpc "Signal expressions must refer to the same MPC controller" + @assert e1.mpc === e2.mpc "Signal expressions must refer to the same MPC" SignalExpr(e1.mpc, [e1.terms; e2.terms]) end -# Unary negation and subtraction for SignalExpr +# Negation and subtraction Base.:-(e::SignalExpr) = - SignalExpr(e.mpc, [SignalTerm(-term.A, term.kind, term.col_idx) for term in e.terms]) -Base.:-(e1::SignalExpr, e2::SignalExpr) = e1 + (-e2) -Base.:-(e::SignalExpr, x::StateRef) = e + (-_bare(x)) -Base.:-(e::SignalExpr, u::ControlRef) = e + (-_bare(u)) -Base.:-(e::SignalExpr, d::DisturbanceRef) = e + (-_bare(d)) -Base.:-(x::StateRef, e::SignalExpr) = _bare(x) + (-e) -Base.:-(u::ControlRef, e::SignalExpr) = _bare(u) + (-e) - -# Allow bare signal + something: x + G*u, u + F*x, etc. -Base.:+(x::StateRef, e::SignalExpr) = _bare(x) + e -Base.:+(e::SignalExpr, x::StateRef) = e + _bare(x) -Base.:+(u::ControlRef, e::SignalExpr) = _bare(u) + e -Base.:+(e::SignalExpr, u::ControlRef) = e + _bare(u) -Base.:+(d::DisturbanceRef, e::SignalExpr) = _bare(d) + e -Base.:+(e::SignalExpr, d::DisturbanceRef) = e + _bare(d) -Base.:+(x::StateRef, u::ControlRef) = _bare(x) + _bare(u) -Base.:+(u::ControlRef, x::StateRef) = _bare(u) + _bare(x) -Base.:+(x::StateRef, d::DisturbanceRef) = _bare(x) + _bare(d) -Base.:+(d::DisturbanceRef, x::StateRef) = _bare(d) + _bare(x) - -# DynamicsExpr: SignalExpr + constant offset vector -Base.:+(e::SignalExpr, offset::AbstractVector) = DynamicsExpr(e, float(offset)) -Base.:+(offset::AbstractVector, e::SignalExpr) = DynamicsExpr(e, float(offset)) -Base.:+(e::DynamicsExpr, offset::AbstractVector) = DynamicsExpr(e.signals, e.offset + offset) -Base.:+(offset::AbstractVector, e::DynamicsExpr) = DynamicsExpr(e.signals, e.offset + offset) -Base.:+(e1::DynamicsExpr, e2::SignalExpr) = DynamicsExpr(e1.signals + e2, e1.offset) -Base.:+(e1::SignalExpr, e2::DynamicsExpr) = DynamicsExpr(e1 + e2.signals, e2.offset) + SignalExpr(e.mpc, [SignalTerm(-t.A, t.kind, t.col_indices) for t in e.terms]) +Base.:-(e1::SignalExpr, e2::SignalExpr) = e1 + (-e2) +Base.:-(e::SignalExpr, x::StateRef) = e + (-_bare(x)) +Base.:-(e::SignalExpr, u::ControlRef) = e + (-_bare(u)) +Base.:-(e::SignalExpr, d::DisturbanceRef) = e + (-_bare(d)) +Base.:-(e::SignalExpr, sv::StateVec) = e + (-_bare(sv)) +Base.:-(e::SignalExpr, cv::ControlVec) = e + (-_bare(cv)) +Base.:-(x::StateRef, e::SignalExpr) = _bare(x) + (-e) +Base.:-(u::ControlRef, e::SignalExpr) = _bare(u) + (-e) +Base.:-(sv::StateVec, e::SignalExpr) = _bare(sv) + (-e) + +# Bare ref + something +for (T, bare_fn) in [(:StateRef, :_bare), (:ControlRef, :_bare), + (:DisturbanceRef, :_bare), (:StateVec, :_bare), + (:ControlVec, :_bare), (:DisturbanceVec, :_bare)] + @eval Base.:+(x::$T, e::SignalExpr) = $bare_fn(x) + e + @eval Base.:+(e::SignalExpr, x::$T) = e + $bare_fn(x) +end +Base.:+(x::StateRef, u::ControlRef) = _bare(x) + _bare(u) +Base.:+(u::ControlRef, x::StateRef) = _bare(u) + _bare(x) +Base.:+(x::StateRef, d::DisturbanceRef) = _bare(x) + _bare(d) +Base.:+(d::DisturbanceRef, x::StateRef) = _bare(d) + _bare(x) +Base.:+(sv::StateVec, cv::ControlVec) = _bare(sv) + _bare(cv) +Base.:+(cv::ControlVec, sv::StateVec) = _bare(cv) + _bare(sv) + +# Support + between Vector{StateRef}/Vector{ControlRef} and SignalExpr +Base.:+(e::SignalExpr, refs::AbstractVector{<:StateRef}) = e + _bare(refs) +Base.:+(refs::AbstractVector{<:StateRef}, e::SignalExpr) = _bare(refs) + e +Base.:+(e::SignalExpr, refs::AbstractVector{<:ControlRef}) = e + _bare(refs) +Base.:+(refs::AbstractVector{<:ControlRef}, e::SignalExpr) = _bare(refs) + e + +# SignalExpr + constant offset vector -> DynamicsExpr +Base.:+(e::SignalExpr, ofs::AbstractVector) = DynamicsExpr(e, float(ofs)) +Base.:+(ofs::AbstractVector, e::SignalExpr) = DynamicsExpr(e, float(ofs)) +Base.:+(e::DynamicsExpr, ofs::AbstractVector) = DynamicsExpr(e.signals, e.offset + ofs) +Base.:+(ofs::AbstractVector, e::DynamicsExpr) = DynamicsExpr(e.signals, e.offset + ofs) +Base.:+(e1::DynamicsExpr, e2::SignalExpr) = DynamicsExpr(e1.signals + e2, e1.offset) +Base.:+(e1::SignalExpr, e2::DynamicsExpr) = DynamicsExpr(e1 + e2.signals, e2.offset) # ---- Model expansion helpers (incremental mode) --------------------------- -""" -Expand mpc.model to add one more state dimension. Returns the new state index. -The new state has zero rows/columns in F, G, Gd and is unobserved in C by default. -""" function _expand_state!(mpc::MPC) m = mpc.model nx, nu, nd = m.nx, m.nu, m.nd new_nx = nx + 1 - F_new = [m.F zeros(nx, 1); - zeros(1, nx) zeros(1, 1)] + F_new = [m.F zeros(nx, 1); zeros(1, nx) zeros(1, 1)] G_new = [m.G; zeros(1, nu)] Gd_new = [m.Gd; zeros(1, nd)] - # Default to full-state output (C = I) in incremental mode - C_new = Matrix{Float64}(I, new_nx, new_nx) + C_new = Matrix{Float64}(I, new_nx, new_nx) # full-state output mpc.model = Model(F_new, G_new; - Gd = Gd_new, - C = C_new, - Dd = m.Dd, - f_offset = [m.f_offset; 0.0], - h_offset = zeros(new_nx), # h_offset grows with ny - xo = [m.xo; 0.0], - uo = m.uo, - wmin = [m.wmin; 0.0], - wmax = [m.wmax; 0.0], - Ts = m.Ts) + Gd=Gd_new, C=C_new, Dd=m.Dd, + f_offset=[m.f_offset; 0.0], h_offset=zeros(new_nx), + xo=[m.xo; 0.0], uo=m.uo, + wmin=[m.wmin; 0.0], wmax=[m.wmax; 0.0], Ts=m.Ts) mpc.mpqp_issetup = false nx_s, nu_s, nd_s = _incremental_registry[mpc] _incremental_registry[mpc] = (nx_s + 1, nu_s, nd_s) - return new_nx # new state index + return new_nx end -""" -Expand mpc.model to add one more control dimension. Returns the new control index. -""" function _expand_control!(mpc::MPC) m = mpc.model - G_new = [m.G zeros(m.nx, 1)] - mpc.model = Model(m.F, G_new; - Gd = m.Gd, - C = m.C, - Dd = m.Dd, - f_offset = m.f_offset, - h_offset = m.h_offset, - xo = m.xo, - uo = [m.uo; 0.0], - wmin = m.wmin, - wmax = m.wmax, - Ts = m.Ts) + mpc.model = Model(m.F, [m.G zeros(m.nx, 1)]; + Gd=m.Gd, C=m.C, Dd=m.Dd, + f_offset=m.f_offset, h_offset=m.h_offset, + xo=m.xo, uo=[m.uo; 0.0], wmin=m.wmin, wmax=m.wmax, Ts=m.Ts) mpc.mpqp_issetup = false nx_s, nu_s, nd_s = _incremental_registry[mpc] _incremental_registry[mpc] = (nx_s, nu_s + 1, nd_s) - return m.nu + 1 # new control index + return m.nu + 1 end -""" -Expand mpc.model to add one more disturbance dimension. Returns the new disturbance index. -""" function _expand_disturbance!(mpc::MPC) m = mpc.model - Gd_new = [m.Gd zeros(m.nx, 1)] - Dd_new = [m.Dd zeros(m.ny, 1)] mpc.model = Model(m.F, m.G; - Gd = Gd_new, - C = m.C, - Dd = Dd_new, - f_offset = m.f_offset, - h_offset = m.h_offset, - xo = m.xo, - uo = m.uo, - wmin = m.wmin, - wmax = m.wmax, - Ts = m.Ts) + Gd=[m.Gd zeros(m.nx,1)], C=m.C, Dd=[m.Dd zeros(m.ny,1)], + f_offset=m.f_offset, h_offset=m.h_offset, + xo=m.xo, uo=m.uo, wmin=m.wmin, wmax=m.wmax, Ts=m.Ts) mpc.mpqp_issetup = false nx_s, nu_s, nd_s = _incremental_registry[mpc] _incremental_registry[mpc] = (nx_s, nu_s, nd_s + 1) - return m.nd + 1 # new disturbance index + return m.nd + 1 end # ---- Internal helpers ------------------------------------------------------ -_bound(::Nothing) = zeros(0) -_bound(x) = x - -_to_vec_bound(::Nothing, n) = zeros(0) -_to_vec_bound(x::AbstractVector, n) = x -_to_vec_bound(x::Number, n) = fill(Float64(x), n) +_bound(::Nothing) = zeros(0) +_bound(x) = x +_to_vec_bound(::Nothing, n) = zeros(0) +_to_vec_bound(x::AbstractVector, n) = x +_to_vec_bound(x::Number, n) = fill(Float64(x), n) _is_signal(::ControlRef) = true _is_signal(::OutputRef) = true _is_signal(::StateRef) = true _is_signal(::DisturbanceRef) = true _is_signal(::SignalExpr) = true +_is_signal(::ControlVec) = true +_is_signal(::StateVec) = true +_is_signal(::DisturbanceVec) = true _is_signal(::Any) = false # ---- Core constraint dispatch ---------------------------------------------- @@ -334,6 +387,10 @@ function _add_constraint!(mpc::MPC, lb, ::ControlRef, ub; kw...) set_input_bounds!(mpc; umin=_bound(lb), umax=_bound(ub)) end +function _add_constraint!(mpc::MPC, lb, ::ControlVec, ub; kw...) + set_input_bounds!(mpc; umin=_bound(lb), umax=_bound(ub)) +end + function _add_constraint!(mpc::MPC, lb, ::OutputRef, ub; ks=2:mpc.Np, soft=true, binary=false, prio=0, kw...) set_output_bounds!(mpc; ymin=_to_vec_bound(lb, mpc.model.ny), @@ -344,9 +401,9 @@ end function _add_constraint!(mpc::MPC, lb, ::StateRef, ub; ks=2:mpc.Np, soft=false, binary=false, prio=0, kw...) add_constraint!(mpc; - Ax = Matrix{Float64}(I, mpc.model.nx, mpc.model.nx), - lb = _to_vec_bound(lb, mpc.model.nx), - ub = _to_vec_bound(ub, mpc.model.nx), + Ax=Matrix{Float64}(I, mpc.model.nx, mpc.model.nx), + lb=_to_vec_bound(lb, mpc.model.nx), + ub=_to_vec_bound(ub, mpc.model.nx), ks=ks, soft=soft, binary=binary, prio=prio) end @@ -356,107 +413,132 @@ end function _add_constraint!(mpc::MPC, lb, expr::SignalExpr, ub; ks=2:mpc.Np, soft=false, binary=false, prio=0, kw...) - Au = zeros(0,0); Ax = zeros(0,0) - Ar = zeros(0,0); Ad = zeros(0,0); Aup = zeros(0,0) - for term in expr.terms - A = term.A - if term.kind == :u; Au = isempty(Au) ? A : Au + A - elseif term.kind == :x; Ax = isempty(Ax) ? A : Ax + A - elseif term.kind == :r; Ar = isempty(Ar) ? A : Ar + A - elseif term.kind == :d; Ad = isempty(Ad) ? A : Ad + A - elseif term.kind == :uprev; Aup = isempty(Aup) ? A : Aup + A + Au=zeros(0,0); Ax=zeros(0,0); Ar=zeros(0,0); Ad=zeros(0,0); Aup=zeros(0,0) + for t in expr.terms + A = t.A + if t.kind == :u; Au = isempty(Au) ? A : Au + A + elseif t.kind == :x; Ax = isempty(Ax) ? A : Ax + A + elseif t.kind == :r; Ar = isempty(Ar) ? A : Ar + A + elseif t.kind == :d; Ad = isempty(Ad) ? A : Ad + A + elseif t.kind == :uprev; Aup = isempty(Aup) ? A : Aup + A end end add_constraint!(mpc; - Au = isempty(Au) ? nothing : Au, - Ax = isempty(Ax) ? nothing : Ax, - Ar = Ar, Ad = Ad, Aup = Aup, - lb = _bound(lb), ub = _bound(ub), + Au=isempty(Au) ? nothing : Au, Ax=isempty(Ax) ? nothing : Ax, + Ar=Ar, Ad=Ad, Aup=Aup, + lb=_bound(lb), ub=_bound(ub), ks=ks, soft=soft, binary=binary, prio=prio) end function _add_constraint_onesided!(mpc::MPC, lhs, op::Symbol, rhs; kw...) if op == :(<=) - if _is_signal(lhs) - _add_constraint!(mpc, nothing, lhs, rhs; kw...) - elseif _is_signal(rhs) - _add_constraint!(mpc, lhs, rhs, nothing; kw...) - else - error("@constraint: no signal reference found in expression") - end + _is_signal(lhs) ? _add_constraint!(mpc, nothing, lhs, rhs; kw...) : + _is_signal(rhs) ? _add_constraint!(mpc, lhs, rhs, nothing; kw...) : + error("@constraint: no signal reference found") elseif op == :(>=) - if _is_signal(lhs) - _add_constraint!(mpc, rhs, lhs, nothing; kw...) - elseif _is_signal(rhs) - _add_constraint!(mpc, nothing, rhs, lhs; kw...) - else - error("@constraint: no signal reference found in expression") - end + _is_signal(lhs) ? _add_constraint!(mpc, rhs, lhs, nothing; kw...) : + _is_signal(rhs) ? _add_constraint!(mpc, nothing, rhs, lhs; kw...) : + error("@constraint: no signal reference found") else error("@constraint: unsupported operator $op") end end -# ---- Macro helper ---------------------------------------------------------- +# ---- Macro helpers --------------------------------------------------------- function _parse_macro_kwargs(args) kw_pairs = Expr[] for arg in args - if arg isa Expr && arg.head == :(=) - push!(kw_pairs, Expr(:kw, arg.args[1], esc(arg.args[2]))) - end + (arg isa Expr && arg.head == :(=)) || continue + push!(kw_pairs, Expr(:kw, arg.args[1], esc(arg.args[2]))) end return kw_pairs end function _make_call(fn, kw_pairs, pos_args...) escaped = [esc(a) for a in pos_args] - if isempty(kw_pairs) - return Expr(:call, fn, escaped...) + isempty(kw_pairs) ? + Expr(:call, fn, escaped...) : + Expr(:call, fn, Expr(:parameters, kw_pairs...), escaped...) +end + +# ---- Dynamics helpers ------------------------------------------------------ + +""" +Apply one `SignalTerm` to a row `r` of matrices `F_new`, `G_new`, `Gd_new`. +Handles dense (`col_indices` empty), single-column, and multi-column terms. +""" +function _apply_term_to_row!(F_new, G_new, Gd_new, term::SignalTerm, r::Int) + A, cols = term.A, term.col_indices + mat = term.kind == :x ? F_new : + term.kind == :u ? G_new : + term.kind == :d ? Gd_new : nothing + mat === nothing && return + if isempty(cols) + # Dense: use row r of A (or row 1 if A has only 1 row) + row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] + mat[r, :] .+= row_vec + elseif length(cols) == 1 + mat[r, cols[1]] += A[1, 1] else - return Expr(:call, fn, Expr(:parameters, kw_pairs...), escaped...) + # Sparse block: A is (1 x ncols) or (nrows x ncols) + row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] + mat[r, cols] .+= row_vec end end -# ---- Dynamics: full-matrix and row-by-row ---------------------------------- +""" +Apply one `SignalTerm` to a block of rows `rows` of F, G, Gd. +""" +function _apply_term_to_rows!(F_new, G_new, Gd_new, term::SignalTerm, rows::Vector{Int}) + A, cols = term.A, term.col_indices + mat = term.kind == :x ? F_new : + term.kind == :u ? G_new : + term.kind == :d ? Gd_new : nothing + mat === nothing && return + n = length(rows) + if isempty(cols) + # Dense: A is (n x ncols) or (1 x ncols) + for (i, r) in enumerate(rows) + row_vec = size(A, 1) == 1 ? vec(A) : A[i, :] + mat[r, :] .+= row_vec + end + elseif length(cols) == 1 + for (i, r) in enumerate(rows) + mat[r, cols[1]] += size(A,1) == 1 ? A[1,1] : A[i,1] + end + else + for (i, r) in enumerate(rows) + row_vec = size(A, 1) == 1 ? vec(A) : A[i, :] + mat[r, cols] .+= row_vec + end + end +end """ _set_dynamics!(mpc, expr) -Set the full F, G, Gd, f_offset from a `SignalExpr` or `DynamicsExpr`. +Replace full F, G, Gd, f_offset from a `SignalExpr` or `DynamicsExpr`. +Preserves C, Dd, h_offset, Ts, operating point. """ function _set_dynamics!(mpc::MPC, expr::SignalExpr) _set_dynamics!(mpc, DynamicsExpr(expr)) end function _set_dynamics!(mpc::MPC, dexpr::DynamicsExpr) - model = mpc.model - nx, nu, nd = model.nx, model.nu, model.nd - - F = zeros(nx, nx) - G = zeros(nx, nu) - Gd = zeros(nx, nd) + m = mpc.model + nx, nu, nd = m.nx, m.nu, m.nd + F = zeros(nx, nx); G = zeros(nx, nu); Gd = zeros(nx, nd) f_offset = isempty(dexpr.offset) ? zeros(nx) : Vector{Float64}(dexpr.offset) - - for term in dexpr.signals.terms - A = term.A - if term.kind == :x; F .+= A - elseif term.kind == :u; G .+= A - elseif term.kind == :d; Gd .+= A + for t in dexpr.signals.terms + if t.kind == :x; F .+= t.A + elseif t.kind == :u; G .+= t.A + elseif t.kind == :d; Gd .+= t.A end end - mpc.model = Model(F, G; - Gd = Gd, - C = model.C, - Dd = model.Dd, - f_offset = f_offset, - h_offset = model.h_offset, - Ts = model.Ts, - xo = model.xo, - uo = model.uo, - wmin = model.wmin, - wmax = model.wmax) + Gd=Gd, C=m.C, Dd=m.Dd, f_offset=f_offset, h_offset=m.h_offset, + Ts=m.Ts, xo=m.xo, uo=m.uo, wmin=m.wmin, wmax=m.wmax) mpc.mpqp_issetup = false return mpc end @@ -464,94 +546,79 @@ end """ _set_dynamics_row!(mpc, row_ref, expr) -Set a single row of F, G, and Gd. `row_ref` must be a `StateRef` with -`idx > 0` (created by `@state` in incremental mode). - -Each `SignalTerm` in `expr` contributes: -- `col_idx == 0` (dense A): if A has one row, that row is used; otherwise row `r` of A. -- `col_idx > 0` (sparse): places scalar `A[1,1]` at the given column. - -Rows not specified via this function remain zero. +Set a single row of F, G, Gd from `expr`. `row_ref` must have `idx > 0`. """ function _set_dynamics_row!(mpc::MPC, row_ref::StateRef, expr) r = row_ref.idx - r > 0 || error("_set_dynamics_row!: expected an indexed StateRef (idx > 0), got idx=0") + r > 0 || error("_set_dynamics_row!: expected indexed StateRef (idx > 0)") m = mpc.model - - F_new = copy(m.F) - G_new = copy(m.G) - Gd_new = copy(m.Gd) - fo_new = copy(m.f_offset) - + F_new = copy(m.F); G_new = copy(m.G); Gd_new = copy(m.Gd); fo_new = copy(m.f_offset) signals = expr isa DynamicsExpr ? expr.signals : expr if expr isa DynamicsExpr && !isempty(expr.offset) ofs = expr.offset fo_new[r] += length(ofs) >= r ? ofs[r] : ofs[end] end + for t in signals.terms + _apply_term_to_row!(F_new, G_new, Gd_new, t, r) + end + mpc.model = Model(F_new, G_new; + Gd=Gd_new, C=m.C, Dd=m.Dd, f_offset=fo_new, h_offset=m.h_offset, + Ts=m.Ts, xo=m.xo, uo=m.uo, wmin=m.wmin, wmax=m.wmax) + mpc.mpqp_issetup = false + return mpc +end - for term in signals.terms - A, col = term.A, term.col_idx - if term.kind == :x - if col == 0 - row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] - F_new[r, :] .+= row_vec - else - F_new[r, col] += A[1, 1] - end - elseif term.kind == :u - if col == 0 - row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] - G_new[r, :] .+= row_vec - else - G_new[r, col] += A[1, 1] - end - elseif term.kind == :d - if col == 0 - row_vec = size(A, 1) == 1 ? vec(A) : A[r, :] - Gd_new[r, :] .+= row_vec - else - Gd_new[r, col] += A[1, 1] - end +""" + _set_dynamics_rows!(mpc, rows, expr) + +Set multiple rows of F, G, Gd simultaneously from `expr`. +`rows` is a `Vector{Int}` of row indices. +""" +function _set_dynamics_rows!(mpc::MPC, rows::Vector{Int}, expr) + m = mpc.model + F_new = copy(m.F); G_new = copy(m.G); Gd_new = copy(m.Gd); fo_new = copy(m.f_offset) + signals = expr isa DynamicsExpr ? expr.signals : expr + if expr isa DynamicsExpr && !isempty(expr.offset) + ofs = expr.offset + for (i, r) in enumerate(rows) + fo_new[r] += length(ofs) >= i ? ofs[i] : 0.0 end end - + for t in signals.terms + _apply_term_to_rows!(F_new, G_new, Gd_new, t, rows) + end mpc.model = Model(F_new, G_new; - Gd = Gd_new, - C = m.C, - Dd = m.Dd, - f_offset = fo_new, - h_offset = m.h_offset, - Ts = m.Ts, - xo = m.xo, - uo = m.uo, - wmin = m.wmin, - wmax = m.wmax) + Gd=Gd_new, C=m.C, Dd=m.Dd, f_offset=fo_new, h_offset=m.h_offset, + Ts=m.Ts, xo=m.xo, uo=m.uo, wmin=m.wmin, wmax=m.wmax) mpc.mpqp_issetup = false return mpc end +# Helper: extract row indices from various LHS types +_dynamics_rows(sv::StateVec) = sv.indices +function _dynamics_rows(v::AbstractVector) + all(x -> x isa StateRef && x.idx > 0, v) || + error("@dynamics: vector LHS must contain only indexed StateRefs") + [x.idx for x in v] +end + # ---- Public macros --------------------------------------------------------- """ @control(mpc, name) - @control(mpc, name, umin=umin_val, umax=umax_val) - -Declare a control input signal reference. + @control(mpc, name[1:n]) + @control(mpc, name, umin=val, umax=val) + @control(mpc, name[1:n], umin=val, umax=val) -**Style 1** (`MPC(F, G; ...)`): assigns a full-vector `ControlRef` with no model change. +Declare control signal reference(s). -**Style 2** (`MPC(Np=...)`): increments `nu` by 1 and assigns an indexed `ControlRef`. +In **incremental mode** (`MPC(Np=...)`): +- `@control mpc u` — adds 1 control; `u` is a `ControlRef`. +- `@control mpc u[1:3]` — adds 3 controls; `u` is a `ControlVec`. -Optionally set input bounds via `umin`/`umax`. - -# Examples -```julia -# Style 1 -mpc = MPC(F, G; Np=10); @control mpc u umin=-1 umax=1 -# Style 2 -mpc = MPC(Np=10); @state mpc x1; @control mpc u -@dynamics mpc x1_next = 0.9*x1 + 0.5*u -``` +In **style-1 mode** (`MPC(F,G;...)`): +- `@control mpc u` — `u` is a full-vector `ControlRef`. +- `@control mpc u[1:3]` — `u` is a `ControlVec` over indices `[1,2,3]`. """ macro control(mpc_ex, name_ex, args...) umin_ex = nothing; umax_ex = nothing @@ -561,52 +628,85 @@ macro control(mpc_ex, name_ex, args...) k == :umin && (umin_ex = arg.args[2]) k == :umax && (umax_ex = arg.args[2]) end + um = isnothing(umin_ex) ? :(zeros(0)) : esc(umin_ex) + uM = isnothing(umax_ex) ? :(zeros(0)) : esc(umax_ex) + apply_bounds = !isnothing(umin_ex) || !isnothing(umax_ex) + + if name_ex isa Expr && name_ex.head == :ref + # Vector form: u[1:n] + var_sym = name_ex.args[1] + range_ex = name_ex.args[2] + result = Expr(:block) + push!(result.args, quote + if LinearMPC._is_incremental($(esc(mpc_ex))) + _n = length($(esc(range_ex))) + _si = $(esc(mpc_ex)).model.nu + 1 + for _ in 1:_n; LinearMPC._expand_control!($(esc(mpc_ex))); end + $(esc(var_sym)) = LinearMPC.ControlVec($(esc(mpc_ex)), collect(_si:_si+_n-1)) + else + $(esc(var_sym)) = LinearMPC.ControlVec($(esc(mpc_ex)), collect($(esc(range_ex)))) + end + end) + apply_bounds && push!(result.args, + :(LinearMPC.set_input_bounds!($(esc(mpc_ex)); umin=$um, umax=$uM))) + push!(result.args, :($(esc(var_sym)))) + return result + end + # Scalar form: u + var_sym = name_ex isa Symbol ? name_ex : error("@control: expected symbol or symbol[range]") result = Expr(:block) push!(result.args, quote if LinearMPC._is_incremental($(esc(mpc_ex))) _idx = LinearMPC._expand_control!($(esc(mpc_ex))) - $(esc(name_ex)) = LinearMPC.ControlRef($(esc(mpc_ex)), _idx) + $(esc(var_sym)) = LinearMPC.ControlRef($(esc(mpc_ex)), _idx) else - $(esc(name_ex)) = LinearMPC.ControlRef($(esc(mpc_ex))) + $(esc(var_sym)) = LinearMPC.ControlRef($(esc(mpc_ex))) end end) - if !isnothing(umin_ex) || !isnothing(umax_ex) - um = isnothing(umin_ex) ? :(zeros(0)) : esc(umin_ex) - uM = isnothing(umax_ex) ? :(zeros(0)) : esc(umax_ex) - push!(result.args, - :(LinearMPC.set_input_bounds!($(esc(mpc_ex)); umin=$um, umax=$uM))) - end - push!(result.args, :($(esc(name_ex)))) + apply_bounds && push!(result.args, + :(LinearMPC.set_input_bounds!($(esc(mpc_ex)); umin=$um, umax=$uM))) + push!(result.args, :($(esc(var_sym)))) return result end """ @state(mpc, name) + @state(mpc, name[1:n]) -Declare a state signal reference. +Declare state signal reference(s). -**Style 1** (`MPC(F, G; ...)`): assigns a full-vector `StateRef` with no model change. +In **incremental mode**: +- `@state mpc x1` — adds 1 state; defines `x1` and `x1_next` (both `StateRef`). +- `@state mpc v[1:4]` — adds 4 states; defines `v` and `v_next` (both `StateVec`). -**Style 2** (`MPC(Np=...)`): increments `nx` by 1 and assigns an indexed `StateRef`. -Also defines `name_next` (e.g., `x1_next`) for use as the LHS of [`@dynamics`](@ref). -Unspecified rows of F, G remain zero. +In **style-1 mode**: +- `@state mpc x` — `x` and `x_next` are full-vector `StateRef`s. +- `@state mpc v[1:4]` — `v` and `v_next` are `StateVec`s over indices `[1,2,3,4]`. -# Examples -```julia -# Style 1 -mpc = MPC(F, G; Np=10); @state mpc x; @constraint mpc -5 <= x <= 5 -# Style 2 -mpc = MPC(Np=10) -@state mpc x1 # defines x1 (StateRef index 1) and x1_next -@state mpc x2 # defines x2 (StateRef index 2) and x2_next -@control mpc u -@dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u -@dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # row 2 of G stays zero -``` +The `_next` variant is provided for readability in `@dynamics` LHS, but the plain +name also works there. """ macro state(mpc_ex, name_ex) - name_ex isa Symbol || error("@state: expected a symbol for the state name") + if name_ex isa Expr && name_ex.head == :ref + var_sym = name_ex.args[1] + range_ex = name_ex.args[2] + next_sym = Symbol(string(var_sym) * "_next") + return quote + if LinearMPC._is_incremental($(esc(mpc_ex))) + _n = length($(esc(range_ex))) + _si = $(esc(mpc_ex)).model.nx + 1 + for _ in 1:_n; LinearMPC._expand_state!($(esc(mpc_ex))); end + $(esc(var_sym)) = LinearMPC.StateVec($(esc(mpc_ex)), collect(_si:_si+_n-1)) + $(esc(next_sym)) = LinearMPC.StateVec($(esc(mpc_ex)), collect(_si:_si+_n-1)) + else + $(esc(var_sym)) = LinearMPC.StateVec($(esc(mpc_ex)), collect($(esc(range_ex)))) + $(esc(next_sym)) = LinearMPC.StateVec($(esc(mpc_ex)), collect($(esc(range_ex)))) + end + end + end + + name_ex isa Symbol || error("@state: expected symbol or symbol[range]") next_sym = Symbol(string(name_ex) * "_next") quote if LinearMPC._is_incremental($(esc(mpc_ex))) @@ -614,7 +714,6 @@ macro state(mpc_ex, name_ex) $(esc(name_ex)) = LinearMPC.StateRef($(esc(mpc_ex)), _idx) $(esc(next_sym)) = LinearMPC.StateRef($(esc(mpc_ex)), _idx) else - # Style 1: no model change; define both name and name_next as full-state refs $(esc(name_ex)) = LinearMPC.StateRef($(esc(mpc_ex))) $(esc(next_sym)) = LinearMPC.StateRef($(esc(mpc_ex))) end @@ -624,37 +723,21 @@ end """ @output(mpc, name) -Declare an output signal reference for use in [`@constraint`](@ref) expressions. - -# Example -```julia -@output mpc y -@constraint mpc 0 <= y <= 5 -``` +Declare an output signal reference. """ macro output(mpc_ex, name_ex) - quote - $(esc(name_ex)) = LinearMPC.OutputRef($(esc(mpc_ex))) - end + quote; $(esc(name_ex)) = LinearMPC.OutputRef($(esc(mpc_ex))); end end """ @disturbance(mpc, name) - @disturbance(mpc, name, wmin=wmin_val, wmax=wmax_val) - -Declare a disturbance signal reference. + @disturbance(mpc, name[1:n]) + @disturbance(mpc, name, wmin=val, wmax=val) -**Style 2** (`MPC(Np=...)`): increments `nd` by 1 and assigns an indexed -`DisturbanceRef`. Also defines `name_next` for use in [`@dynamics`](@ref). - -# Example -```julia -@disturbance mpc d wmin=-0.1*ones(1) wmax=0.1*ones(1) -``` +Declare disturbance signal reference(s). Like `@state` and `@control`, supports +both scalar (`d`) and vector (`d[1:2]`) forms in both style-1 and incremental mode. """ macro disturbance(mpc_ex, name_ex, args...) - name_ex isa Symbol || error("@disturbance: expected a symbol for the disturbance name") - next_sym = Symbol(string(name_ex) * "_next") wmin_ex = nothing; wmax_ex = nothing for arg in args (arg isa Expr && arg.head == :(=)) || continue @@ -662,7 +745,35 @@ macro disturbance(mpc_ex, name_ex, args...) k == :wmin && (wmin_ex = arg.args[2]) k == :wmax && (wmax_ex = arg.args[2]) end + wm = isnothing(wmin_ex) ? :(zeros(0)) : esc(wmin_ex) + wM = isnothing(wmax_ex) ? :(zeros(0)) : esc(wmax_ex) + apply_bounds = !isnothing(wmin_ex) || !isnothing(wmax_ex) + + if name_ex isa Expr && name_ex.head == :ref + var_sym = name_ex.args[1] + range_ex = name_ex.args[2] + next_sym = Symbol(string(var_sym) * "_next") + result = Expr(:block) + push!(result.args, quote + if LinearMPC._is_incremental($(esc(mpc_ex))) + _n = length($(esc(range_ex))) + _si = $(esc(mpc_ex)).model.nd + 1 + for _ in 1:_n; LinearMPC._expand_disturbance!($(esc(mpc_ex))); end + $(esc(var_sym)) = LinearMPC.DisturbanceVec($(esc(mpc_ex)), collect(_si:_si+_n-1)) + $(esc(next_sym)) = LinearMPC.DisturbanceVec($(esc(mpc_ex)), collect(_si:_si+_n-1)) + else + $(esc(var_sym)) = LinearMPC.DisturbanceVec($(esc(mpc_ex)), collect($(esc(range_ex)))) + $(esc(next_sym)) = LinearMPC.DisturbanceVec($(esc(mpc_ex)), collect($(esc(range_ex)))) + end + end) + apply_bounds && push!(result.args, + :(LinearMPC.set_disturbance!($(esc(mpc_ex)), $wm, $wM))) + push!(result.args, :($(esc(var_sym)))) + return result + end + name_ex isa Symbol || error("@disturbance: expected symbol or symbol[range]") + next_sym = Symbol(string(name_ex) * "_next") result = Expr(:block) push!(result.args, quote if LinearMPC._is_incremental($(esc(mpc_ex))) @@ -673,12 +784,8 @@ macro disturbance(mpc_ex, name_ex, args...) $(esc(name_ex)) = LinearMPC.DisturbanceRef($(esc(mpc_ex))) end end) - if !isnothing(wmin_ex) || !isnothing(wmax_ex) - wm = isnothing(wmin_ex) ? :(zeros(0)) : esc(wmin_ex) - wM = isnothing(wmax_ex) ? :(zeros(0)) : esc(wmax_ex) - push!(result.args, - :(LinearMPC.set_disturbance!($(esc(mpc_ex)), $wm, $wM))) - end + apply_bounds && push!(result.args, + :(LinearMPC.set_disturbance!($(esc(mpc_ex)), $wm, $wM))) push!(result.args, :($(esc(name_ex)))) return result end @@ -687,30 +794,13 @@ end @constraint(mpc, lb <= signal <= ub) @constraint(mpc, signal <= ub) @constraint(mpc, signal >= lb) - @constraint(mpc, lb <= A*signal1 + B*signal2 <= ub) @constraint(mpc, expr, soft=true, ks=2:5, binary=false, prio=0) -Add a constraint to the MPC controller `mpc`. - -| Signal type | Calls | Default `soft` | -|:-------------|:---------------------------|:---------------| -| `ControlRef` | `set_input_bounds!` | N/A | -| `OutputRef` | `set_output_bounds!` | `true` | -| `StateRef` | `add_constraint!` (Ax = I) | `false` | -| `SignalExpr` | `add_constraint!` | `false` | - -# Examples -```julia -@constraint mpc -1 <= u <= 1 -@constraint mpc 0 <= y <= 5 -@constraint mpc u <= 0.5 -@constraint mpc lb <= Au*u + Ax*x <= ub -@constraint mpc -1 <= u <= 1 soft=true ks=3:8 -``` +Add a constraint. Works with `ControlRef`, `ControlVec`, `OutputRef`, `StateRef`, +`DisturbanceRef`, and `SignalExpr`. """ macro constraint(mpc_ex, expr, args...) kw_pairs = _parse_macro_kwargs(args) - if expr isa Expr && expr.head == :comparison && length(expr.args) == 5 lb_ex, op1, mid_ex, op2, ub_ex = expr.args if op1 == :(<=) && op2 == :(<=) @@ -721,68 +811,76 @@ macro constraint(mpc_ex, expr, args...) mpc_ex, ub_ex, mid_ex, lb_ex) end end - if expr isa Expr && expr.head == :call && length(expr.args) == 3 op, lhs_ex, rhs_ex = expr.args - if op in (:(<=), :(>=)) + op in (:(<=), :(>=)) && return _make_call(:(LinearMPC._add_constraint_onesided!), kw_pairs, mpc_ex, lhs_ex, QuoteNode(op), rhs_ex) - end end - - return :(error("@constraint: unsupported expression `" * string($(QuoteNode(expr))) * "`\n" * - "Expected: lb <= signal <= ub, signal <= ub, or signal >= lb")) + return :(error("@constraint: unsupported expression `" * string($(QuoteNode(expr))) * "`")) end """ - @dynamics(mpc, x_next = F*x + G*u) - @dynamics(mpc, x_next = F*x + G*u + Gd*d + f_offset) - @dynamics(mpc, x1_next = 0.9*x1 - 0.2*x2 + 0.5*u1) + @dynamics(mpc, lhs = rhs) Set (or update) system dynamics. -**Style 1** (full-matrix): the LHS `x_next` is a `StateRef` with `idx == 0` -(created by `@state mpc x` on a dimensioned MPC). The RHS provides F, G, Gd, -and an optional constant offset vector all at once. +**LHS forms:** +- `x_next` or `x` — single `StateRef` with `idx > 0`; sets one row of F, G. +- `v[3]` — index into `StateVec`; sets the row for `v.indices[3]`. +- `v[1:2]` or `[x; z]` — `StateVec` or vector of `StateRef`s; sets multiple rows. +- `x_next` with `idx==0` — full-matrix assignment (style 1). -**Style 2** (row-by-row): the LHS `x1_next` is an indexed `StateRef` (created -by `@state mpc x1` on an incremental `MPC(Np=...)`). The RHS expression -defines *one row* of F and G using scalar or row-vector coefficients. -Calling `@dynamics` for each state sets the corresponding row; rows not -explicitly set remain zero. +**RHS forms:** +- `F*x + G*u` — style 1 full-matrix. +- `0.9*x1 - 0.2*x2 + 0.5*u` — scalar coefficient terms (style 2 single row). +- `A*v[1:3] + B*u` — matrix times `StateVec`/`ControlVec` (style 2 multi-row). -The C, Dd, h_offset, Ts, and operating-point settings of the existing model -are always preserved. +Undefined rows of F, G remain zero. # Examples ```julia -# Style 1 -- full matrices -mpc = MPC(2, 1; Np=10) -@state mpc x; @control mpc u +# Style 1 +mpc = MPC(2, 1; Np=10); @state mpc x; @control mpc u A = [1.0 0.1; 0.0 1.0]; B = [0.0; 1.0] @dynamics mpc x_next = A*x + B*u -# Style 2 -- row by row (incremental) +# Style 2 -- scalar mpc = MPC(Np=10) @state mpc x1; @state mpc x2; @control mpc u -@dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u # row 1 of F and G -@dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # row 2; G[2,:] stays 0 +@dynamics mpc x1 = 0.9*x1 - 0.2*x2 + 0.5*u # row 1 (can use x1 or x1_next) +@dynamics mpc x2 = 0.1*x1 + 0.8*x2 # row 2 + +# Style 2 -- vector +@state mpc v[1:3]; @control mpc u +A3 = rand(3,3) +@dynamics mpc v = A3*v + 0.5*u[1] # sets rows for all elements of v -# With disturbance and affine offset (style 2) -@disturbance mpc d -@dynamics mpc x1_next = 0.9*x1 + 0.5*u + 0.1*d +# Style 2 -- vcat LHS +@state mpc x1; @state mpc x2 +@dynamics mpc [x1; x2] = A*[x1; x2] # alternative multi-row form ``` """ macro dynamics(mpc_ex, eq_ex) (eq_ex isa Expr && eq_ex.head == :(=)) || - error("@dynamics: expected an assignment expression, e.g. `x1_next = 0.9*x1 + 0.5*u`") + error("@dynamics: expected assignment, e.g. `x1 = 0.9*x1 + 0.5*u`") lhs = eq_ex.args[1] rhs = eq_ex.args[2] return quote let _lhs_val = $(esc(lhs)), _rhs_val = $(esc(rhs)) if _lhs_val isa LinearMPC.StateRef && _lhs_val.idx > 0 + # Single row (scalar StateRef, includes x1 and x1_next) LinearMPC._set_dynamics_row!($(esc(mpc_ex)), _lhs_val, _rhs_val) + elseif _lhs_val isa LinearMPC.StateVec + # Multiple rows via StateVec (e.g. v, v[1:2]) + LinearMPC._set_dynamics_rows!($(esc(mpc_ex)), + LinearMPC._dynamics_rows(_lhs_val), _rhs_val) + elseif _lhs_val isa AbstractVector + # Multiple rows via [x1; x2] vcat + LinearMPC._set_dynamics_rows!($(esc(mpc_ex)), + LinearMPC._dynamics_rows(_lhs_val), _rhs_val) else + # Full-matrix (style 1) LinearMPC._set_dynamics!($(esc(mpc_ex)), _rhs_val) end end @@ -792,18 +890,10 @@ end """ @objective(mpc, Q=Q_val, R=R_val, Rr=Rr_val, S=S_val, Qf=Qf_val, Qfx=Qfx_val) -Set the objective function weights for the MPC controller. -Equivalent to [`set_objective!`](@ref)`(mpc; Q=Q_val, R=R_val, ...)`. - -# Example -```julia -using LinearAlgebra -@objective mpc Q=I R=0.1*I Rr=0.01*I -``` +Set the objective function weights. """ macro objective(mpc_ex, args...) - kw_pairs = _parse_macro_kwargs(args) - return _make_call(:(LinearMPC.set_objective!), kw_pairs, mpc_ex) + _make_call(:(LinearMPC.set_objective!), _parse_macro_kwargs(args), mpc_ex) end # ---- MPC constructor for incremental mode ---------------------------------- @@ -811,24 +901,18 @@ end """ MPC(; Np=10, Nc=Np) -Create a zero-dimensional MPC controller in **incremental mode**. - -States, controls, and disturbances are added one at a time using the -[`@state`](@ref), [`@control`](@ref), and [`@disturbance`](@ref) macros. -Each `@state mpc x_i` call increments the state dimension by 1 and defines -both `x_i` (for RHS use) and `x_i_next` (for the LHS of [`@dynamics`](@ref)). - -Dynamics are specified row-by-row via [`@dynamics`](@ref); undefined rows -default to zero. +Create a zero-dimensional MPC in **incremental mode**. States, controls, and +disturbances are added one at a time (or in batches) with [`@state`](@ref), +[`@control`](@ref), and [`@disturbance`](@ref). Dynamics are set row-by-row +with [`@dynamics`](@ref); undefined rows default to zero. # Example ```julia mpc = MPC(Np=10) -@state mpc x1; @state mpc x2 -@control mpc u +@state mpc x1; @state mpc x2; @control mpc u -@dynamics mpc x1_next = 0.9*x1 - 0.2*x2 + 0.5*u -@dynamics mpc x2_next = 0.1*x1 + 0.8*x2 # G[2,:] stays zero +@dynamics mpc x1 = 0.9*x1 - 0.2*x2 + 0.5*u +@dynamics mpc x2 = 0.1*x1 + 0.8*x2 @objective mpc Q=I R=0.1 @constraint mpc -1 <= u <= 1