FAst ConTrollEr SynThesis (FACTEST) is a framework for controller synthesis for nonlinear systems in complex environments. You can find links to related publications here. The controller synthesis problem requires an agent, such as a mobile robot or a quadcopter, to reach a goal (green set) starting from a set of initial configurations (blue), while avoiding obstacles (red).
This repository serves as the accompanying artifact evaluation package for the CAV2020 paper.
FACTEST has been tested with Python3 on Ubuntu 16.04 and macOS 10.15 (Catalina).
The following libraries are used in FACTEST. You may have to use the --user option in the following.
- Yices - installation instructions can be found here. For macOS first install
brew install SRI-CSL/sri-csl/yices2thenpip3 install yices. - TuLiP Polytope
pip3 install polytope - Pypoman
pip3 install pypoman - NumPy
pip3 install numpy - SciPy
pip3 install scipy - Matplotlib
pip3 install matplotlib
This articact can be run by either downloading the zip folder for this repository or by downloading the VM. If you choose to run the artifact by downloading the zip file, be sure that all the requirements are installed. Everything is run from the root folder.
The VM can be downloaded from https://uofi.box.com/v/FACTESTDownload. The instructions for using the VM can be found at https://uofi.box.com/v/FACTESTInstructions. The username for the VM is FACTEST and the password for the VM is factest2020.
Once the VM is downloaded, open the terminal and use the following command:
cd Desktop\FACTEST
In table 1, there is a list of scenarios that FACTEST is tested on. If you would like to run all the FACTEST scenarios, then run the following command:
python3 factest_all.py --plot [bool]
This will run all the scenarios. The results for all the scenarios will be saved to a file called results/synthesis-results/all_data.csv. If --plot True is used, then each of the figures will be saved to a figure called results/figures/modelname_envname.png. Note that plotting may take a few minutes. The default value of --plot is False.
To run these scenarios, navigate to the FACTEST folder and run the FACTEST python file using the following command:
python3 factest.py [modelname] [envname] --segs [int] --plot [bool]
The models that run in the 2D workspace are car and robot. The models that run in the 3D workspace are hovercraft and auv.
The environments in the 2D workspace are zigzag1, zigzag2, zigzag3, barrier, maze, and SCOTS. The environments in the 3D worspace are ztunnel and ltunnel.
This gives the maximum number of line segments to find a path. In table 1, the maximum number of line segments in 10 for every scenario except the SCOTS scenario. For this scenario, the maximum number of line segments should be set to 100. If unspecified, the maximum number of line segments will default to 100.
This specifies whether or not the scenario will be plotted. If True, then a plot showing the scenario, reference trajectory, and actual trajectory will be shown.
The results of the scenario will be saved to a file called results/synthesis-results/modelname_envname.csv. If you choose to plot the scenario, then it will be saved to a figure called results/figures/modelname_envname.png.
For example, if you want to save the results of the car model in the maze environment, with 10 maximum line segments and no plot, the following command would be run:
python3 factest.py car maze --segs 10 --plot False
The results from this scenario will be saved in a file called car_maze.csv.
The results from figure 5 can can also be replicated. In order to do this, run the following command:
python3 path_comparison.py --env [envname] --plot [bool]
The environments in the 2D workspace are zigzag1, zigzag2, zigzag3, barrier, maze, and SCOTS. The environments in the 3D worspace are ztunnel and ltunnel.
If no env is specified or if envname is chosen to be all, then all the scenarios are run. Note that it takes more than 15 minutes to run all the scenarios.
This specifies whether or not the results will be plotted. If True, then a plot comparing the iterations and time for the scenario will be created. These plots will be saved to results/figures/envname_time_comparison.png and results/figures/envname_iter_comparison.png
The SCOTS scenario is not run using RRT since it would take over an hour to run the file. We didn't report the runtime for the SCOTS scenario using RRT.
The results of the scenarios are saved to two files called results/RRTvSAT-Plan/RRT_envname.csv and results/RRTvSAT-Plan/yices_envname.csv.
The results reported are shown below:
| Model | Env | Initial Set Size | # Obstacles | Time | # Partitions |
|---|---|---|---|---|---|
| car | zigzag1 | 0.200 | 9 | 0.028 | 1 |
| car | zigzag2 | 0.400 | 9 | 0.144 | 4 |
| car | zigzag3 | 0.600 | 9 | 0.605 | 16 |
| car | maze | 0.200 | 22 | 0.078 | 1 |
| car | barrier | 0.707 | 6 | 0.415 | 22 |
| car | SCOTS | 0.070 | 19 | 0.667 | 1 |
| robot | zigzag1 | 0.200 | 9 | 0.025 | 1 |
| robot | zigzag2 | 0.400 | 9 | 0.196 | 4 |
| robot | zigzag3 | 0.600 | 9 | 0.612 | 16 |
| robot | maze | 0.200 | 22 | 0.079 | 1 |
| robot | barrier | 0.707 | 6 | 0.498 | 22 |
| robot | SCOTS | 0.070 | 19 | 0.635 | 1 |
| auv | ztunnel | 0.866 | 6 | 0.372 | 1 |
| auv | ltunnel | 0.866 | 8 | 0.317 | 1 |
| hovercraft | ztunnel | 0.866 | 6 | 0.472 | 1 |
| hovercraft | ltunnel | 0.866 | 8 | 0.140 | 1 |
The results from the RRT vs. SAT-Plan are shown below:
Up to now you can replicate all results reported in the paper.
You can create your own scenarios using FACTEST. Either a new environment can be created or a new vehicle model can be added. If a new environment is created, any of the existing models can be used as long as the workspaces match (i.e. ground vehicle models on 2D workspaces and aerial/underwater vehicle models on 3D workspaces). If a new model is created, then any of the existing environments can be used as long as the workspaces match. After the new scenario is created, merely import the files into the FACTEST file to run. Note that it is much simpler to create new 2D workspace environments.
New environments can be created for FACTEST. To create an environment, the following functions are necessary:
problem(): This function returns the obstacles, initial set, and goal as a triple(obstacles, Theta, goal). The obstacles, initial set, and goal set are all represented as polytopes. A polytope{x | Ax <= b}is represented as a pair(A, b).
The obstacles are given as a list of polytopes [(Ao1, bo1), ... , (Aon, bon)]. The inital set is given as (A0, b0), and the goal set is given as (Af, gf). Note that the initial set must be a rectangle in order for the partitioning to work. The example from the barrier environment is shown below:
import numpy as np
from math import *
def problem():
A_rect = np.array([[-1,0],
[1,0],
[0,-1],
[0,1]])
# Since all the obstacles are rectangles,
# the A-matrix is the same for each polytope
b1 = np.array([[-1.55], [1.9], [-0], [2.0]])
b2 = np.array([[-1], [2.5], [-2.6], [3.0]])
b3 = np.array([[0], [4.0], [.1], [0]])
b4 = np.array([[0], [4.0], [-4.0], [4.1]])
b5 = np.array([[-4.0], [4.1], [0], [4.0]])
b6 = np.array([[.1], [0], [0], [4.0]])
obstacles = [(A_rect, b1),
(A_rect, b2),
(A_rect, b3),
(A_rect, b4),
(A_rect, b5),
(A_rect, b6)]
b0 = np.array([[-0.5], [1.5], [-1.5], [2.5]])
Theta = (A_rect, b0)
# The initial set must be a rectangle
# for the partitioning to work
bf = np.array([[-3], [3.675], [-3], [3.675]])
goal = (A_rect, bf)
return obstacles, Theta, goal
This will generate an environment as the following, where obstacles are red regions and regions outside the borders, initial set is the blue region, and the goal set is the green region:
plot_problem()(3D Workspace only): This is only required for plotting scenarios in the 3D workspace since it is difficult to see the scenarios with many obstacle polytopes. This function will plot the safe space (complementary of the obstacles) that the system must travel through. Examples can be seen inLtunnel.pyandztunnel.pyin theenvsfolder. Note that you do not need this function if you do not need to plot the 3D Workspace scenarios.
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
from util.plot_polytope3d import *
import numpy as np
def plot_problem():
fig = plt.figure()
axes = fig.add_subplot(111, projection='3d')
obstacles, Theta, Goal = problem()
A_cube = np.array([[-1, 0, 0],
[1, 0, 0],
[0, -1, 0],
[0, 1, 0],
[0, 0, -1],
[0, 0, 1]])
b1 = np.array([[0], [15], [0], [5], [0], [5]])
b2 = np.array([[-10], [15], [0], [5], [-5], [20]])
b3 = np.array([[-10], [30], [0], [5], [-20], [25]])
b4 = np.array([[-25], [30], [0], [5], [-5], [20]])
b5 = np.array([[-25], [40], [0], [5], [0], [5]])
# These polytopes correlate to polytopes that fit in
# the safe space of the l-tunnel
if axes == None:
axes = a3.Axes3D(plt.figure())
plot_polytope_3d(A_cube, b1, ax = axes, color = 'yellow', trans = 0.1)
plot_polytope_3d(A_cube, b2, ax = axes, color = 'yellow', trans = 0.1)
plot_polytope_3d(A_cube, b3, ax = axes, color = 'yellow', trans = 0.1)
plot_polytope_3d(A_cube, b4, ax = axes, color = 'yellow', trans = 0.1)
plot_polytope_3d(A_cube, b5, ax = axes, color = 'yellow', trans = 0.1)
plot_polytope_3d(A_cube, Theta[1], ax = axes, color = 'blue', trans = 0.9)
plot_polytope_3d(A_cube, Goal[1], ax = axes, color = 'green', trans = 0.9)
axes.set_xlabel('x')
axes.set_ylabel('y')
axes.set_zlabel('z')
# The axes must be returned since the reference and actual
# trajectories should be plotted on top of the environment
return axes
This will generate an environment as the following, where the safe space (complimentary of the obstacles) is the yellow cubes, initial set is the blue cube, and the goal set is the green cube:
- Once the above function(s) is created, they can be imported to FACTEST.py (added in the
Import the environmentpart) to run. The following shows how to add the barrier example, which is stored in theenvfolder aspartition2.pyto the main algorithm:
elif args.env == 'barrier':
from envs.partition2 import *
dim = 2 # The workspace dimension is 2
When creating a new model to be used with factest, there are four functions that must exist:
model(q, t, u): This function contains the dynamics of the nonlinear vehicle model. The input to the function is: the stateq, the timet, and the input to the systemu. The function returns the derivative of the state variables inqwith respect to time. In other words, themodel(q, t, u)contains the ODE for the vehicular model. The car model example is shown below:
def model(q, t, u):
x, y, theta = q
v, w = u
dxdt = v*cos(theta)
dydt = v*sin(theta)
dthetadt = w
return [dxdt, dydt, dthetadt]
controller(q, qref, uref): This function contains the control law for the Lyapunov based feedback controller. The input to the function is: the current state of the systemq, the reference stateqref, and the reference inputuref. The output is the input to the system.
Each state in qref is given by [x, y, theta] for the 2D scenarios, and [x, y, z, roll, pitch, yaw] for the 3D scenarios. Each input in uref is given by [v, w] for the 2D scenarios and [v, [wx, wy, wz]] for the 3D scenarios. These states and inputs can be converted to the states and inputs needed for your system. For example, the robot model has the states x, y, cos(theta), sin(theta). In the controller, the states given by qref are converted to the necessary states.
The Lyapunov based feedback controller for the car example is shown below:
def controller(q, qref, uref):
x, y, theta = q
xref, yref, thetaref = qref
vref, wref = uref
# These controller parameter are defined globally
k1, k2, k3 = k
# Compute the error
xe = cos(theta)*(xref - x) + sin(theta)*(yref - y)
ye = -sin(theta)*(xref - x) + cos(theta)*(yref - y)
thetae = thetaref - theta
# Control law
v = vref*cos(thetae) + k1*xe
w = wref + vref*(k2*ye + k3*sin(thetae))
return [v, w]
bloating(n, alpha): This is the function that computes the amount that the size of each obstacle is increased by as each line segment is added. The equation used in this section is computed from the Lyapunov based feedback controller (see Lemma 2 and Lemma 3 in the paper). The way that this equation is explained in Section 4.3. The number of the line segment is given byn+1and the size of the initial sublevel set given by the initial set isalpha. The function returns a number that the obstacles are increased by. The example for the car model is shown below:
def bloating(n, alpha):
k1, k2, k3 = k
if n != 0 and n != 1:
return sqrt(alpha**2 + 4*n/k2)
else:
return sqrt(alpha**2 + 4/k2)
run_model(): This function runs the model that you created using the reference controller computed by FACTEST. The inputs to the function are: the initial stateq0, the array of timestampst, the array of reference statesqref, and the array of reference inputsuref. The output is the solution to the system.
def run_model(q0, t, qref, uref):
q = [q0]
u0 = [0, 0]
for i in range(0,len(t)):
t_step = np.linspace(t[i-1], t[i], 11)
q1 = odeint(model, q0, t_step, args = (u0,))
q0 = q1[1]
q.append(q0)
u0 = controller(q0, qref[i], uref[i])
return q