Skip to content

Commit 149c05c

Browse files
johnaogajohnaoga
committed
format
1 parent dfea7f3 commit 149c05c

File tree

2 files changed

+53
-61
lines changed

2 files changed

+53
-61
lines changed

‎docs/src/examples/solvers/Path planning sgmpc.jl

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@ using Test #src
4040
# - $x_r = (\sqrt{32}/3, \sqrt{20}/3)$
4141
#
4242

43-
4443
# First, let us import [StaticArrays](https://github.com/JuliaArrays/StaticArrays.jl) and [Plots](https://github.com/JuliaPlots/Plots.jl).
4544
using StaticArrays, Plots
4645

@@ -70,7 +69,8 @@ target = SVector(-0.5, 0.5, -pi)
7069
#target = SVector(sqrt(32.0 / 3.0), sqrt(20.0 / 3.0), -pi)
7170

7271
# Now we instantiate the problem using the function provided by [PathPlanning.jl](@__REPO_ROOT_URL__/problems/PathPlanningSgMPC.jl)
73-
concrete_problem = PathPlanningSgMPC.problem(; sgmpc = true, initial = initial, target = target)
72+
concrete_problem =
73+
PathPlanningSgMPC.problem(; sgmpc = true, initial = initial, target = target)
7474
concrete_system = concrete_problem.system;
7575

7676
# ### Definition of the abstraction

‎problems/path_planning_sgmpc.jl

Lines changed: 51 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -8,40 +8,38 @@ const DO = Dionysos.Domain
88
const PB = Dionysos.Problem
99
const ST = Dionysos.System
1010

11-
1211
# TODO: Move to Dionysos.Utils (the function is already there, but it imposes that Q, R, and N must be the same type and *size* and that q and r should be the same type and *size*)
13-
struct QuadraticStateControlFunction{T} <:
14-
UT.ScalarControlFunction
15-
Q::AbstractMatrix{T}
16-
R::AbstractMatrix{T}
17-
N::AbstractMatrix{T}
18-
q::AbstractArray{T}
19-
r::AbstractArray{T}
20-
v::T
21-
22-
#=
23-
function QuadraticStateControlFunction(Q::MT, R::MT, N::MT, q::AT, r::AT, v::T) where {T, MT <: AbstractMatrix{T}, AT <: AbstractArray{T}}
24-
# Perform size checks
25-
@assert size(Q, 1) == size(Q, 2) "Q must be square"
26-
@assert size(R, 1) == size(R, 2) "R must be square"
27-
@assert size(Q, 1) == size(N, 1) "Q and N must have compatible dimensions"
28-
@assert size(R, 1) == size(N, 2) "R and N must have compatible dimensions"
29-
@assert size(Q, 1) == length(q) "Q and q must have compatible dimensions"
30-
@assert size(R, 1) == length(r) "R and r must have compatible dimensions"
31-
new{T, MT, AT}(Q, R, N, q, r, v)
32-
end
33-
=#
12+
struct QuadraticStateControlFunction{T} <: UT.ScalarControlFunction
13+
Q::AbstractMatrix{T}
14+
R::AbstractMatrix{T}
15+
N::AbstractMatrix{T}
16+
q::AbstractArray{T}
17+
r::AbstractArray{T}
18+
v::T
19+
20+
#=
21+
function QuadraticStateControlFunction(Q::MT, R::MT, N::MT, q::AT, r::AT, v::T) where {T, MT <: AbstractMatrix{T}, AT <: AbstractArray{T}}
22+
# Perform size checks
23+
@assert size(Q, 1) == size(Q, 2) "Q must be square"
24+
@assert size(R, 1) == size(R, 2) "R must be square"
25+
@assert size(Q, 1) == size(N, 1) "Q and N must have compatible dimensions"
26+
@assert size(R, 1) == size(N, 2) "R and N must have compatible dimensions"
27+
@assert size(Q, 1) == length(q) "Q and q must have compatible dimensions"
28+
@assert size(R, 1) == length(r) "R and r must have compatible dimensions"
29+
new{T, MT, AT}(Q, R, N, q, r, v)
30+
end
31+
=#
3432

3533
end
3634
function function_value(f::QuadraticStateControlFunction, x, u)
37-
return x'f.Q * x + u'f.R * u + 2 * (x'f.N * u + x'f.q + u'f.r) + f.v
35+
return x'f.Q * x + u'f.R * u + 2 * (x'f.N * u + x'f.q + u'f.r) + f.v
3836
end
3937
function get_full_psd_matrix(f::QuadraticStateControlFunction)
40-
return [
41-
f.Q f.N f.q
42-
f.N' f.R f.r
43-
f.q' f.r' f.v
44-
]
38+
return [
39+
f.Q f.N f.q
40+
f.N' f.R f.r
41+
f.q' f.r' f.v
42+
]
4543
end
4644
##########
4745

@@ -115,9 +113,9 @@ function system(
115113
udim = 2,
116114
sysnoise = SVector(0.0, 0.0, 0.0),
117115
measnoise = SVector(0.0, 0.0, 0.0),
118-
tstep = 1.,
116+
tstep = 1.0,
119117
nsys = 5,
120-
ngrowthbound = 5
118+
ngrowthbound = 5,
121119
)
122120
sys_map = let nsys = nsys
123121
(x, u, _) -> SVector{3}(
@@ -134,13 +132,9 @@ function system(
134132
)
135133
end
136134
growthbound_map = let ngrowthbound = ngrowthbound
137-
(r, u, _) -> SVector{3}(
138-
u[1] * r[3],
139-
u[1] * r[3],
140-
0.
141-
)
135+
(r, u, _) -> SVector{3}(u[1] * r[3], u[1] * r[3], 0.0)
142136
end
143-
contsys = ST.ControlSystemGrowth(
137+
contsys = ST.ControlSystemGrowth(
144138
tstep,
145139
sysnoise,
146140
measnoise,
@@ -178,26 +172,23 @@ L(x) = 100 |(x_1, x_2)^T - x_r|^2
178172
179173
These costs are defined by the quadratic form `x' * Q * x + u' * R * u + 2 * (x' * N * u + x' * q + u' * r) + v`.
180174
"""
181-
function problem(; sgmpc = false, initial = SVector(1.0, -1.7, 0.0), target = SVector(0.5, 0.5, -pi))
175+
function problem(;
176+
sgmpc = false,
177+
initial = SVector(1.0, -1.7, 0.0),
178+
target = SVector(0.5, 0.5, -pi),
179+
)
182180
_X_ = UT.HyperRectangle(SVector(-3.5, -2.6, -pi), SVector(3.5, 2.6, pi))
183181
_I_ = UT.HyperRectangle(initial, initial + SVector(0.2, 0.2, 0.0))
184-
_T_ = UT.HyperRectangle(target, target + SVector(0.2, 0.2, 2*pi))
182+
_T_ = UT.HyperRectangle(target, target + SVector(0.2, 0.2, 2 * pi))
185183

186184
obs = get_obstacles(_X_)
187185
obstacles_LU = filter_obstacles(_X_, _I_, _T_, obs)
188186
_X_ = UT.LazySetMinus(_X_, obstacles_LU)
189187
sys = system(_X_)
190188

191189
if sgmpc
192-
Q = SMatrix{3, 3}(
193-
100.0, 0.0, 0.0,
194-
0.0, 100.0, 0.0,
195-
0.0, 0.0, 0.0,
196-
)
197-
R = SMatrix{2, 2}(
198-
1.0, 0.0,
199-
0.0, 1.0,
200-
)
190+
Q = SMatrix{3, 3}(100.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 0.0)
191+
R = SMatrix{2, 2}(1.0, 0.0, 0.0, 1.0)
201192
N = zeros(Float64, 3, 2)
202193

203194
target_without_last_element = SVector{3}(1.0, 1.0, 0.0) .* target
@@ -206,19 +197,20 @@ function problem(; sgmpc = false, initial = SVector(1.0, -1.7, 0.0), target = SV
206197
v = 100 * target_without_last_element' * target_without_last_element
207198
NT = 20
208199

209-
zero_cost = QuadraticStateControlFunction(zeros(Float64, 3, 3), zeros(Float64, 2, 2), zeros(Float64, 3, 2), [0.0, 0.0, 0.0], [0.0, 0.0], 0.0)
210-
state_cost = [QuadraticStateControlFunction(Q, R, N, q, r, v) for _ in 1:(NT-1)]
211-
terminal_cost = [ zero_cost for _ in 1:NT]
212-
terminal_cost[NT] = QuadraticStateControlFunction(Q, zeros(Float64, 2, 2), N, q, r, v)
213-
214-
problem = PB.OptimalControlProblem(
215-
sys,
216-
_I_,
217-
_T_,
218-
state_cost,
219-
terminal_cost,
220-
NT,
200+
zero_cost = QuadraticStateControlFunction(
201+
zeros(Float64, 3, 3),
202+
zeros(Float64, 2, 2),
203+
zeros(Float64, 3, 2),
204+
[0.0, 0.0, 0.0],
205+
[0.0, 0.0],
206+
0.0,
221207
)
208+
state_cost = [QuadraticStateControlFunction(Q, R, N, q, r, v) for _ in 1:(NT - 1)]
209+
terminal_cost = [zero_cost for _ in 1:NT]
210+
terminal_cost[NT] =
211+
QuadraticStateControlFunction(Q, zeros(Float64, 2, 2), N, q, r, v)
212+
213+
problem = PB.OptimalControlProblem(sys, _I_, _T_, state_cost, terminal_cost, NT)
222214
else
223215
#TODO: Convert LazySetMinus to HyperRectangle to use SafetyProblem
224216
#problem = PB.SafetyProblem(sys, sys.X, sys.X, PB.Infinity())

0 commit comments

Comments
 (0)