WIP-mobile-robot-unicycle

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

@@ -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.

problems/path_planning_sgmpc.jl

@@ -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