# SafeFeedbackMotionPlanning

*This package is under active development*

Design safe controllers for nonlinear systems in the presence of disturbances. The controllers ensure that the system state remains in a tunable *safe tube* around the desired state. Find out more about it in our paper:

```
@article{lakshmanan2020safe,
title={Safe Feedback Motion Planning: A Contraction Theory and $\mathcal{L}_1$-Adaptive Control Based Approach},
author={Lakshmanan, Arun and Gahlawat, Aditya and Hovakimyan, Naira},
journal={arXiv preprint arXiv:2004.01142},
year={2020}
}
```

## Problem Formulation

Consider a system of the form

`ẋ = f(x) + B(x)u`

where `x(t), f(x) ∈ ℝⁿ`

, `u(t) ∈ ℝᵐ`

, and `B(x) ∈ ℝⁿˣᵐ`

, that is facing disturbances `h(t,x) ∈ ℝᵐ`

(either/both state and time dependent) that is matched with the control channel:

`ẋ = f(x) + B(x)(u + h(t,x))`

Given a desired state-control trajectory pair `(x*,u*)`

(feasible under the nominal system) that the actual system is required to follow, design a controller `u(t)`

such that the states remain inside a tube `Ω(ρ, x*(t)) = {ℝⁿ : ||y - x*(t)|| ≤ ρ}`

of some user defined width `ρ > 0`

.

In this package you can design controllers to do exactly this! A more in-depth review of the control architecture can be found in the paper.

## Usage

### Installation

`julia> ] add https://github.com/arlk/SafeFeedbackMotionPlanning.jl`

### Example

```
using SafeFeedbackMotionPlanning
using DifferentialEquations
using Plots
using LinearAlgebra
# Define system matrices or functions
f(x) = [-x[1] + 2*x[2];
-0.25*x[2]^3 - 3*x[1] + 4*x[2]]
B = [0.5, -2.0]
# Simulation time span
tspan = (0., 5.)
# Intial condition of the system
x0 = [1.0, -1.0]
# Define desired regulation point or trajectories (as functions)
xs = [0.0, 0.0]
us = 0.0
# Dual of the control contraction metric matrix (or as a function if state-dependent)
W = [4.25828 -0.93423; -0.93423 3.76692]
λ = 1.74
# Uncertainty unknown to the controller
h(t, x) = -2*sin(2*t) -0.01*x'*x
# Upper bound on the norm of the uncertainty
# for any x ∈ Ω(ρ = √2) and t ≥ 0 (see paper for more details).
Δh = 2.1
ω = 90
Γ = 4e7
# Construct objects using the parameters you just defined
sys_p = sys_params(f, B)
ccm_p = ccm_params(xs, us, λ, W)
l1_p = l1_params(ω, Γ, Δh)
# CCM only system without perturbations
nom_sys = nominal_system(sys_p, ccm_p)
nom_sol = solve(nom_sys, x0, tspan, Tsit5(), progress = true, progress_steps = 1)
# CCM only system with perturbations
ptb_sys = nominal_system(sys_p, ccm_p, h)
ptb_sol = solve(ptb_sys, x0, tspan, Tsit5(), progress = true, progress_steps = 1)
# L1 Reference sysem with perturbations AKA (non-implementable) ideal
# performance of the L1 system with full knowledge of the uncertainty
ref_sys = reference_system(sys_p, ccm_p, ω, h)
ref_sol = solve(ref_sys, x0, tspan, Tsit5(), progress = true, progress_steps = 1)
# L1 + CCM system with perturbations
l1_sys = l1_system(sys_p, ccm_p, l1_p, h)
l1_sol = solve(l1_sys, x0, tspan, Rosenbrock23(), progress = true, progress_steps = 1, saveat = 0.01)
l = @layout [a b; c d]
p1 = plot(nom_sol, vars=[(0,1),(0,2)], title="Ideal CCM performance", legend=:none)
p2 = plot(ptb_sol, vars=[(0,1),(0,2)], title="CCM with perturbations", legend=:none)
p3 = plot(ref_sol, vars=[(0,1),(0,2)], title="Ideal L1+CCM with perturbations\n (non-implementable)", legend=:none)
p4 = plot(l1_sol, vars=[(0,1),(0,2)], title="L1+CCM with perturbations", legend=:none)
plot(p1, p2, p3, p4, layout=l)
```

## TODO

- CCM metric search using SumofSquares.jl
- Automatically computing ω and Γ from the tube bounds