-
Notifications
You must be signed in to change notification settings - Fork 16
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
WIP-mobile-robot-unicycle #354
Draft
johnaoga
wants to merge
24
commits into
master
Choose a base branch
from
WIP-mobile-robot-unicycle-antoine-girard
base: master
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Draft
Changes from 4 commits
Commits
Show all changes
24 commits
Select commit
Hold shift + click to select a range
83ada73
init
johnaoga add973b
fixing the obstables function
johnaoga 7910ff9
cleaning
johnaoga 3011a92
Finer abstraction
adrienbanse 7a18ac5
progress
johnaoga 7304427
Merge branch 'WIP-mobile-robot-unicycle-antoine-girard' of github.com…
johnaoga 82e4607
Fixing uncovered initial set failure
johnaoga bcdff7c
Finer abstraction
adrienbanse 00a5f76
Fixing uncovered initial set failure
johnaoga 2092447
jformat
johnaoga 1e1fa91
merge...
johnaoga 106619f
Merge branch 'master' into WIP-mobile-robot-unicycle-antoine-girard
adrienbanse f10a0d6
Merge branch 'master' into WIP-mobile-robot-unicycle-antoine-girard
adrienbanse 709c818
fix: init set lower and upper bounds
adrienbanse 1090d1d
fix: target set
adrienbanse 85b3969
fix: control origin and initial state
adrienbanse d66c144
clean: i -> _
adrienbanse 6ef3430
fix: system definition
adrienbanse 42b878e
clean: remove approx_mode
adrienbanse c8da1e7
a bit cleaning
johnaoga 9ee82a3
adding the optimal function
johnaoga d07adf3
adding check
johnaoga dfea7f3
progress
johnaoga 149c05c
format
johnaoga File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,132 @@ | ||
using Test #src | ||
# # Example: Path planning problem solved by [Uniform grid abstraction](https://github.com/dionysos-dev/Dionysos.jl/blob/master/docs/src/manual/manual.md#solvers). | ||
# | ||
#md # [![Binder](https://mybinder.org/badge_logo.svg)](@__BINDER_ROOT_URL__/generated/Path planning.ipynb) | ||
#md # [![nbviewer](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/generated/Path planning.ipynb) | ||
# | ||
# This example was borrowed from [1, IX. Examples, A] whose dynamics comes from the model given in [2, Ch. 2.4]. | ||
# This is a **reachability problem** for a **continuous system**. | ||
# | ||
# Let us consider the 3-dimensional state space control system of the form | ||
# ```math | ||
# \dot{x} = f(x, u) | ||
# ``` | ||
# with $f: \mathbb{R}^3 × U ↦ \mathbb{R}^3$ given by | ||
# ```math | ||
# f(x,(u_1,u_2)) = \begin{bmatrix} u_1 \cos(α+x_3)\cos(α^{-1}) \\ u_1 \sin(α+x_3)\cos(α^{-1}) \\ u_1 \tan(u_2) \end{bmatrix} | ||
# ``` | ||
# and with $U = [−1, 1] \times [−1, 1]$ and $α = \arctan(\tan(u_2)/2)$. Here, $(x_1, x_2)$ is the position and $x_3$ is the | ||
# orientation of the vehicle in the 2-dimensional plane. The control inputs $u_1$ and $u_2$ are the rear | ||
# wheel velocity and the steering angle. | ||
# The control objective is to drive the vehicle which is situated in a maze made of obstacles from an initial position to a target position. | ||
# | ||
# | ||
# In order to study the concrete system and its symbolic abstraction in a unified framework, we will solve the problem | ||
# for the sampled system with a sampling time $\tau$. | ||
# For the construction of the relations in the abstraction, it is necessary to over-approximate attainable sets of | ||
# a particular cell. In this example, we consider the used of a growth bound function [1, VIII.2, VIII.5] which is one of the possible methods to over-approximate | ||
# attainable sets of a particular cell based on the state reach by its center. | ||
# | ||
# For this reachability problem, the abstraction controller is built by solving a fixed-point equation which consists in computing the pre-image | ||
# of the target set. | ||
|
||
# First, let us import [StaticArrays](https://github.com/JuliaArrays/StaticArrays.jl) and [Plots](https://github.com/JuliaPlots/Plots.jl). | ||
using StaticArrays, Plots | ||
|
||
# At this point, we import Dionysos. | ||
using Dionysos | ||
const DI = Dionysos | ||
const UT = DI.Utils | ||
const DO = DI.Domain | ||
const ST = DI.System | ||
const SY = DI.Symbolic | ||
const PR = DI.Problem | ||
const OP = DI.Optim | ||
const AB = OP.Abstraction | ||
|
||
# And the file defining the hybrid system for this problem | ||
include(joinpath(dirname(dirname(pathof(Dionysos))), "problems", "path_planning_sgmpc.jl")) | ||
|
||
# ### Definition of the problem | ||
|
||
# Now we instantiate the problem using the function provided by [PathPlanning.jl](@__REPO_ROOT_URL__/problems/PathPlanningSgMPC.jl) | ||
concrete_problem = PathPlanningSgMPC.problem(; simple = true, approx_mode = PathPlanningSgMPC.GROWTH); | ||
concrete_system = concrete_problem.system; | ||
|
||
# ### Definition of the abstraction | ||
|
||
# Definition of the grid of the state-space on which the abstraction is based (origin `x0` and state-space discretization `h`): | ||
x0 = SVector(0.0, 0.0, 0.0); | ||
h = SVector(0.1, 0.1, 0.2); | ||
state_grid = DO.GridFree(x0, h); | ||
|
||
# Definition of the grid of the input-space on which the abstraction is based (origin `u0` and input-space discretization `h`): | ||
u0 = SVector(0.0, 0.0); | ||
h = SVector(0.3, 0.3); | ||
input_grid = DO.GridFree(u0, h); | ||
|
||
# We now solve the optimal control problem with the `Abstraction.UniformGridAbstraction.Optimizer`. | ||
|
||
using JuMP | ||
optimizer = MOI.instantiate(AB.UniformGridAbstraction.Optimizer) | ||
MOI.set(optimizer, MOI.RawOptimizerAttribute("concrete_problem"), concrete_problem) | ||
MOI.set(optimizer, MOI.RawOptimizerAttribute("state_grid"), state_grid) | ||
MOI.set(optimizer, MOI.RawOptimizerAttribute("input_grid"), input_grid) | ||
MOI.optimize!(optimizer) | ||
|
||
# Get the results | ||
abstract_system = MOI.get(optimizer, MOI.RawOptimizerAttribute("abstract_system")) | ||
abstract_problem = MOI.get(optimizer, MOI.RawOptimizerAttribute("abstract_problem")) | ||
abstract_controller = MOI.get(optimizer, MOI.RawOptimizerAttribute("abstract_controller")) | ||
concrete_controller = MOI.get(optimizer, MOI.RawOptimizerAttribute("concrete_controller")) | ||
|
||
##@test length(abstract_controller.data) == 19400 #src | ||
|
||
# ### Trajectory display | ||
# We choose a stopping criterion `reached` and the maximal number of steps `nsteps` for the sampled system, i.e. the total elapsed time: `nstep`*`tstep` | ||
# as well as the true initial state `x0` which is contained in the initial state-space `_I_` defined previously. | ||
nstep = 100 | ||
function reached(x) | ||
if x ∈ concrete_problem.target_set | ||
return true | ||
else | ||
return false | ||
end | ||
end | ||
x0 = SVector(0.5, 0.5, 0.0) | ||
control_trajectory = ST.get_closed_loop_trajectory( | ||
concrete_system.f, | ||
concrete_controller, | ||
x0, | ||
nstep; | ||
stopping = reached, | ||
) | ||
|
||
# Here we display the coordinate projection on the two first components of the state space along the trajectory. | ||
fig = plot(; aspect_ratio = :equal); | ||
# We display the concrete domain | ||
plot!(concrete_system.X; color = :yellow, opacity = 0.5); | ||
|
||
# We display the abstract domain | ||
plot!(abstract_system.Xdom; color = :blue, opacity = 0.5); | ||
|
||
# We display the concrete specifications | ||
plot!(concrete_problem.initial_set; color = :green, opacity = 0.2); | ||
plot!(concrete_problem.target_set; dims = [1, 2], color = :red, opacity = 0.2); | ||
|
||
# We display the abstract specifications | ||
plot!( | ||
SY.get_domain_from_symbols(abstract_system, abstract_problem.initial_set); | ||
color = :green, | ||
); | ||
plot!( | ||
SY.get_domain_from_symbols(abstract_system, abstract_problem.target_set); | ||
color = :red, | ||
); | ||
|
||
# We display the concrete trajectory | ||
plot!(control_trajectory; ms = 0.5) | ||
|
||
# ### References | ||
# 1. G. Reissig, A. Weber and M. Rungger, "Feedback Refinement Relations for the Synthesis of Symbolic Controllers," in IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1781-1796. | ||
# 2. K. J. Aström and R. M. Murray, Feedback systems. Princeton University Press, Princeton, NJ, 2008. |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,199 @@ | ||
module PathPlanningSgMPC | ||
|
||
using StaticArrays | ||
using MathematicalSystems, HybridSystems | ||
using Dionysos | ||
const UT = Dionysos.Utils | ||
const DO = Dionysos.Domain | ||
const PB = Dionysos.Problem | ||
const ST = Dionysos.System | ||
|
||
@enum ApproxMode GROWTH LINEARIZED | ||
|
||
function dynamicofsystem() | ||
# System eq x' = F_sys(x, u) | ||
function F_sys(x, u) | ||
return SVector{3}( | ||
x[1] + u[1] * cos(x[3]), | ||
x[2] + u[1] * sin(x[3]), | ||
x[3] + u[2] % (2 * π), | ||
) | ||
end | ||
|
||
# We define the growth bound function of $f$: | ||
ngrowthbound = 5 | ||
|
||
# We define the growth bound function of $f$: | ||
# FIXME: Double check if this is need and is the correct growth bound | ||
function L_growthbound(u) | ||
beta = abs(u[1]) # We assume that the velocity is bounded by 1? | ||
gamma = abs(u[2]) # We assume that the angle is bounded by 2π | ||
return SMatrix{3, 3}(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, beta, beta, gamma) | ||
end | ||
|
||
# We define the linearized system of $f$: | ||
# DF(x, u) = ∂f/∂x(x, u) | ||
function DF_sys(x, u) | ||
return SMatrix{3, 3}( | ||
1.0, | ||
0.0, | ||
-u[1] * cos(x[3]), | ||
0.0, | ||
1.0, | ||
-u[1] * sin(x[3]), | ||
0.0, | ||
0.0, | ||
1, | ||
) | ||
end | ||
|
||
# We define the bound of the linearized system of $f$: | ||
bound_DF(u) = 0.0 | ||
bound_DDF(u) = 0.0 | ||
|
||
return F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF | ||
end | ||
|
||
function filter_obstacles(_X_, _I_, _T_, obs) | ||
obstacles = typeof(_X_)[] | ||
for ob in obs | ||
if ob ⊆ _X_ && isempty(ob ∩ _I_) && isempty(ob ∩ _T_) | ||
push!(obstacles, ob) | ||
end | ||
end | ||
obstacles_LU = UT.LazyUnionSetArray(obstacles) | ||
return obstacles_LU | ||
end | ||
|
||
function extract_rectangles(matrix) | ||
if isempty(matrix) | ||
return [] | ||
end | ||
|
||
n, m = size(matrix) | ||
tlx, tly, brx, bry = Int[], Int[], Int[], Int[] | ||
|
||
# Build histogram heights | ||
for i in 1:n | ||
j = 1 | ||
while j <= m | ||
if matrix[i, j] == 1 | ||
j += 1 | ||
continue | ||
end | ||
push!(tlx, j) | ||
push!(tly, i) | ||
while j <= m && matrix[i, j] == 0 | ||
j += 1 | ||
end | ||
push!(brx, j - 1) | ||
push!(bry, i) | ||
end | ||
end | ||
|
||
return zip(tlx, tly, brx, bry) | ||
end | ||
|
||
|
||
function get_obstacles( | ||
_X_; | ||
lb_x1 = -3.5, | ||
ub_x1 = 3.5, | ||
lb_x2 = -2.6, | ||
ub_x2 = 2.6, | ||
h = 0.1, | ||
) | ||
# Define the obstacles | ||
x1 = range(lb_x1, stop = ub_x1, step = h) | ||
x2 = range(lb_x2, stop = ub_x2, step = h) | ||
steps1, steps2 = length(x1), length(x2) | ||
|
||
X1 = x1' .* ones(steps2) | ||
X2 = ones(steps1)' .* x2 | ||
|
||
Z1 = (X1 .^ 2 .- X2 .^ 2) .<= 4 | ||
Z2 = (4 .* X2 .^ 2 .- X1 .^ 2) .<= 16 | ||
|
||
|
||
# Find the upper and lower bounds of X1 and X2 for the obstacle | ||
grid = Z1 .& Z2 | ||
|
||
return [ | ||
UT.HyperRectangle(SVector(x1[x1lb], x2[x2lb], _X_.lb[3]), SVector(x1[x1ub], x2[x2ub], _X_.ub[3])) | ||
for (x1lb, x2lb, x1ub, x2ub) in extract_rectangles(grid) | ||
] | ||
|
||
end | ||
|
||
function system( | ||
_X_; | ||
_U_ = UT.HyperRectangle(SVector(0.2, -1.0), SVector(2.0, 1.0)), | ||
xdim = 3, | ||
udim = 2, | ||
sysnoise = SVector(0.0, 0.0, 0.0), | ||
measnoise = SVector(0.0, 0.0, 0.0), | ||
tstep = 0.3, | ||
nsys = 5, | ||
approx_mode::ApproxMode = GROWTH, | ||
) | ||
F_sys, L_growthbound, ngrowthbound, DF_sys, bound_DF, bound_DDF = dynamicofsystem() | ||
contsys = nothing | ||
if approx_mode == GROWTH | ||
contsys = ST.NewControlSystemGrowthRK4( | ||
tstep, | ||
F_sys, | ||
L_growthbound, | ||
sysnoise, | ||
measnoise, | ||
nsys, | ||
ngrowthbound, | ||
) | ||
elseif approx_mode == LINEARIZED | ||
contsys = ST.NewControlSystemLinearizedRK4( | ||
tstep, | ||
F_sys, | ||
DF_sys, | ||
bound_DF, | ||
bound_DDF, | ||
measnoise, | ||
nsys, | ||
) | ||
end | ||
return MathematicalSystems.ConstrainedBlackBoxControlContinuousSystem( | ||
contsys, | ||
xdim, | ||
udim, | ||
_X_, | ||
_U_, | ||
) | ||
end | ||
|
||
"""" | ||
problem() | ||
|
||
This function create the system with `PathPlanningSgMPC.system`. | ||
|
||
Then, we define initial and target domains for the state of the system. | ||
|
||
Finally, we instantiate our Reachability Problem as an OptimalControlProblem | ||
with the system, the initial and target domains, and null cost functions. | ||
""" | ||
function problem(; simple = false, approx_mode::ApproxMode = GROWTH) | ||
if simple | ||
_X_ = UT.HyperRectangle(SVector(-3.5, -2.6, -pi), SVector(3.5, 2.6, pi)) | ||
_I_ = UT.HyperRectangle(SVector(1.0, -1.5, 0.0), SVector(1.0, -1.5, 0.0)) | ||
_T_ = UT.HyperRectangle(SVector(0.5, 0.5, 0.0), SVector(0.5, 0.5, 0.0)) | ||
else | ||
_X_ = UT.HyperRectangle(SVector(-3.5, -2.6, -pi), SVector(3.5, 2.6, pi)) | ||
_I_ = UT.HyperRectangle(SVector(1.0, -1.5, 0.0), SVector(1.0, -1.5, 0.0)) | ||
_T_ = UT.HyperRectangle(SVector(sqrt(32.0 / 3.0), sqrt(20.0 / 3.0), 0.0), SVector(sqrt(32.0 / 3.0), sqrt(20.0 / 3.0), 0.0)) | ||
end | ||
obs = get_obstacles(_X_) | ||
obstacles_LU = filter_obstacles(_X_, _I_, _T_, obs) | ||
_X_ = UT.LazySetMinus(_X_, obstacles_LU) | ||
sys = system(_X_; approx_mode = approx_mode) | ||
problem = PB.OptimalControlProblem(sys, _I_, _T_, nothing, nothing, PB.Infinity()) | ||
return problem | ||
end | ||
|
||
end |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@johnaoga cc my e-mail, just revert if you want to go back to the coarser abstraction
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you! No worries!