From 06bdc20b3cb1553eca4595665ea6693567de435a Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 5 Dec 2024 10:50:58 +0100
Subject: [PATCH] fixed submodules to not be submodules

---
 .../path_generation/star_navigation           |   1 -
 .../path_generation/star_navigation/README.md |  18 +
 .../star_navigation/config/__init__.py        |   0
 .../star_navigation/config/load_config.py     |  83 ++
 .../config/params/dmp_ctrl_params.yaml        |  16 +
 .../pfmpc_artificial_reference_params.yaml    |  40 +
 .../config/params/pfmpc_ds_params.yaml        |  42 +
 .../pfmpc_obstacle_constraints_params.yaml    |  35 +
 .../config/params/robot_params.yaml           |  34 +
 .../config/params/soads_ctrl_params.yaml      |  14 +
 .../star_navigation/config/scene.py           | 769 ++++++++++++++++++
 .../motion_control/__init__.py                |   0
 .../motion_control/dmp/__init__.py            |   1 +
 .../star_navigation/motion_control/dmp/dmp.py | 175 ++++
 .../pfmpc_artificial_reference/__init__.py    |   2 +
 .../motion_controller.py                      | 142 ++++
 .../pfmpc_artificial_reference/mpc.py         | 489 +++++++++++
 .../mpc_build/.gitignore                      |   4 +
 .../motion_control/pfmpc_ds/__init__.py       |   5 +
 .../pfmpc_ds/motion_controller.py             | 447 ++++++++++
 .../motion_control/pfmpc_ds/mpc.py            | 364 +++++++++
 .../pfmpc_ds/mpc_build/.gitignore             |   4 +
 .../motion_control/pfmpc_ds/path_generator.py | 110 +++
 .../pfmpc_ds/workspace_modification.py        | 139 ++++
 .../pfmpc_obstacle_constraints/__init__.py    |   2 +
 .../motion_controller.py                      | 138 ++++
 .../pfmpc_obstacle_constraints/mpc.py         | 398 +++++++++
 .../mpc_build/.gitignore                      |   4 +
 .../motion_control/soads/__init__.py          |   1 +
 .../motion_control/soads/soads.py             | 386 +++++++++
 .../star_navigation/robot/__init__.py         |   7 +
 .../star_navigation/robot/bicycle.py          |  55 ++
 .../star_navigation/robot/mobile_robot.py     |  74 ++
 .../star_navigation/robot/omnidirectional.py  |  23 +
 .../star_navigation/robot/unicycle.py         |  68 ++
 .../star_navigation/scripts/compute_time.py   | 135 +++
 .../star_navigation/scripts/run_simulation.py | 304 +++++++
 .../scripts/simulate_several_controllers.py   | 158 ++++
 .../scripts/simulate_several_initpos.py       | 190 +++++
 .../star_navigation/scripts/test_soads.py     | 161 ++++
 .../path_generation/star_navigation/setup.py  |  19 +
 .../star_navigation.egg-info/PKG-INFO         |   3 +
 .../star_navigation.egg-info/SOURCES.txt      |  51 ++
 .../dependency_links.txt                      |   1 +
 .../star_navigation.egg-info/top_level.txt    |   4 +
 .../star_navigation/visualization/__init__.py |   2 +
 .../visualization/figures/.gitignore          |   4 +
 .../visualization/scene_gui.py                | 383 +++++++++
 .../visualization/video_writer.py             |  45 +
 .../visualization/videos/.gitignore           |   4 +
 .../path_generation/starworld_tunnel_mpc      |   1 -
 .../starworld_tunnel_mpc/README.md            |  22 +
 .../starworld_tunnel_mpc/config/__init__.py   |   0
 .../config/load_config.py                     |  79 ++
 .../config/params/robot_params.yaml           |  27 +
 .../config/params/soads_ctrl_params.yaml      |  14 +
 .../params/tunnel_mpc_convergence_params.yaml |  39 +
 .../config/params/tunnel_mpc_params.yaml      |  38 +
 .../starworld_tunnel_mpc/config/scene.py      | 555 +++++++++++++
 .../motion_control/__init__.py                |   0
 .../motion_control/soads/__init__.py          |   1 +
 .../motion_control/soads/soads.py             | 260 ++++++
 .../motion_control/tunnel_mpc/__init__.py     |   4 +
 .../tunnel_mpc/mpc_build/.gitignore           |   4 +
 .../tunnel_mpc/path_generator.py              | 108 +++
 .../motion_control/tunnel_mpc/tunnel_mpc.py   | 377 +++++++++
 .../tunnel_mpc/tunnel_mpc_controller.py       | 101 +++
 .../tunnel_mpc/workspace_modification.py      |  66 ++
 .../tunnel_mpc_convergence/__init__.py        |   4 +
 .../mpc_build/.gitignore                      |   4 +
 .../tunnel_mpc_convergence/path_generator.py  | 108 +++
 .../tunnel_mpc_convergence/tunnel_mpc.py      | 386 +++++++++
 .../tunnel_mpc_controller.py                  | 107 +++
 .../workspace_modification.py                 |  66 ++
 .../starworld_tunnel_mpc/robot/__init__.py    |   7 +
 .../starworld_tunnel_mpc/robot/bicycle.py     |  55 ++
 .../robot/mobile_robot.py                     |  62 ++
 .../robot/omnidirectional.py                  |  23 +
 .../starworld_tunnel_mpc/robot/unicycle.py    |  38 +
 .../starworld_tunnel_mpc/setup.py             |  15 +
 .../tests/test_motion_control.py              |  75 ++
 .../starworld_tunnel_mpc/tests/test_soads.py  | 176 ++++
 .../visualization/__init__.py                 |   1 +
 .../visualization/scene_gui.py                | 182 +++++
 84 files changed, 8557 insertions(+), 2 deletions(-)
 delete mode 160000 python/ur_simple_control/path_generation/star_navigation
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/README.md
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/load_config.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/config/scene.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py
 create mode 100755 python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py
 create mode 100755 python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/setup.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py
 create mode 100644 python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore
 delete mode 160000 python/ur_simple_control/path_generation/starworld_tunnel_mpc
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py
 create mode 100755 python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py
 create mode 100755 python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py
 create mode 100644 python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py

diff --git a/python/ur_simple_control/path_generation/star_navigation b/python/ur_simple_control/path_generation/star_navigation
deleted file mode 160000
index 2e24e2f..0000000
--- a/python/ur_simple_control/path_generation/star_navigation
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 2e24e2ffca8a7662146e90f06fce74f40af6a316
diff --git a/python/ur_simple_control/path_generation/star_navigation/README.md b/python/ur_simple_control/path_generation/star_navigation/README.md
new file mode 100644
index 0000000..07a9481
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/README.md
@@ -0,0 +1,18 @@
+# star_navigation
+
+## Installation
+Clone repo and submodule
+```
+git clone --recur https://github.com/albindgit/star_navigation.git
+```
+Create and activate virtual environment
+```
+cd star_navigation
+python -m venv venv
+. venv/bin/activate
+```
+Install package
+```
+pip install -e .
+pip install -e starworlds
+```
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/__init__.py b/python/ur_simple_control/path_generation/star_navigation/config/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/load_config.py b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
new file mode 100644
index 0000000..0858ef1
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/load_config.py
@@ -0,0 +1,83 @@
+import yaml
+import pathlib
+from config.scene import Scene
+from motion_control.soads import SoadsController
+from motion_control.dmp import DMPController
+from motion_control.pfmpc_ds import MotionController as PFMPC_ds
+from motion_control.pfmpc_obstacle_constraints import MotionController as PFMPC_obstacle_constraints
+from motion_control.pfmpc_artificial_reference import MotionController as PFMPC_artificial_reference
+from robot import Unicycle, Omnidirectional, Bicycle
+import numpy as np
+
+
+def load_config(scene_id=None, robot_type_id=None, ctrl_param_file=None, verbosity=0):
+    param_path = pathlib.PurePath(__file__).parents[0].joinpath('params')
+
+
+    # Load robot
+    robot_param_path = str(param_path.joinpath('robot_params.yaml'))
+    with open(r'' + robot_param_path) as stream:
+        params = yaml.safe_load(stream)
+    if robot_type_id is None:
+        print("Select robot ID\n -------------")
+        for i, k in enumerate(params.keys()):
+            print(str(i) + ": " + k)
+        robot_type_id = int(input("-------------\nRobot type: "))
+    robot_type = list(params.keys())[robot_type_id]
+    robot_params = params[robot_type]
+
+    scene = Scene(scene_id, robot_radius=robot_params['radius'])
+
+    if robot_params['model'] == 'Omnidirectional':
+        robot = Omnidirectional(radius=robot_params['radius'],
+                                vel_max=robot_params['vel_max'],
+                                name=robot_type)
+        x0 = scene.p0
+    elif robot_params['model'] == 'Unicycle':
+        robot = Unicycle(radius=robot_params['radius'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
+                         vel_max=[robot_params['lin_vel_max'], robot_params['ang_vel_max']],
+                         name=robot_type)
+        try:
+            x0 = np.append(scene.p0, [scene.theta0])
+        except AttributeError:
+            x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])])
+    elif robot_params['model'] == 'Bicycle':
+        robot = Bicycle(radius=robot_params['radius'],
+                        vel_min=[robot_params['lin_vel_min'], -robot_params['steer_vel_max']],
+                        vel_max=[robot_params['lin_vel_max'], robot_params['steer_vel_max']],
+                        steer_max=robot_params['steer_max'],
+                        name=robot_type)
+        try:
+            x0 = np.append(scene.p0, [scene.theta0, 0])
+            # x0 = np.append(scene.p0, [scene.theta0])
+        except AttributeError:
+            x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0]), 0])
+            # x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])])
+    else:
+        raise Exception("[Load Config]: Invalid robot model.\n"
+                        "\t\t\tSelection: {}\n"
+                        "\t\t\tValid selections: [Omnidirectional, Unicycle, Bicycle]".format(robot_params['model']))
+
+    # Load control parameters
+    ctrl_param_path = str(param_path.joinpath(ctrl_param_file))
+    # param_file = str(pathlib.PurePath(pathlib.Path(__file__).parent, ctrl_param_file))
+    with open(r'' + ctrl_param_path) as stream:
+        params = yaml.safe_load(stream)
+    if 'soads' in params:
+        controller = SoadsController(params['soads'], robot, verbosity)
+    elif 'dmp' in params:
+        controller = DMPController(params['dmp'], robot, scene.reference_path, verbosity)
+    elif 'pfmpc_ds' in params:
+        # if global_path is None:
+        #     global_path = [scene.p0.tolist(), scene.pg.tolist()]
+        controller = PFMPC_ds(params['pfmpc_ds'], robot, scene.reference_path, verbosity)
+    elif 'pfmpc_obstacle_constraints' in params:
+        controller = PFMPC_obstacle_constraints(params['pfmpc_obstacle_constraints'], robot, scene.reference_path, verbosity)
+    elif 'pfmpc_artificial_reference' in params:
+        controller = PFMPC_artificial_reference(params['pfmpc_artificial_reference'], robot, scene.reference_path, verbosity)
+    else:
+        raise Exception("[Load Config]: No valid controller selection in param file.\n"
+                        "\t\t\tSelection: {}\n"
+                        "\t\t\tValid selections: [soads, tunnel_mpc, pfmpc_ds]".format(str(list(params.keys())[0])))
+
+    return scene, robot, controller, x0
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml
new file mode 100644
index 0000000..7785e8d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/params/dmp_ctrl_params.yaml
@@ -0,0 +1,16 @@
+dmp:
+  K: 100
+  D: 20
+  n_bfs: 1000
+
+  dt: 0.01
+  starify: 1
+  hull_epsilon: 0.5
+  max_compute_time: 1000
+  make_convex: 0
+  adapt_obstacle_velocity: 0
+  crep: 2.5
+  reactivity: 1.2
+  tail_effect: 0
+  convergence_tolerance: 0.01
+  dp_decay_rate: 0.5
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml
new file mode 100644
index 0000000..344b3bd
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_artificial_reference_params.yaml
@@ -0,0 +1,40 @@
+
+pfmpc_artificial_reference:
+  rhrp_steps: 24
+  # MPC
+#  Q: [100, 100, 1]
+  R: [0.1, .1]
+#  S: [0.01, 0.01]
+  S: [0.01, 0.01]
+#  T: 1
+  K: 0.5
+  mu: 5.e+5
+
+
+  Q: [10, 10, 10]
+#  R: [10, 10]
+#  R: [0, 10]
+  T: 10
+
+  obstacle_padding: 0.0
+
+  #-------- MPC Build params --------- #
+  build_mode: 'release'
+  integration_method: 'RK4'
+  n_pol: 6
+  N: 12
+  dt: 0.2
+  max_No_ell: 10
+  N_obs_predict: 12
+  max_No_pol: 10
+  max_No_vert: 10
+  xaeN_con: 0
+  dlp_con: 0
+  solver_tol: 1.e-5
+  solver_max_time: 500 # Maximum duration for mpc solver in milliseconds
+  solver_max_inner_iterations: 1000
+  solver_max_outer_iterations: 10
+  solver_initial_tol: 1.e-4
+  solver_delta_tol: 1.e-4
+  solver_weight_update_factor: 10.0
+  solver_initial_penalty: 1000.0
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml
new file mode 100644
index 0000000..06d3214
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_ds_params.yaml
@@ -0,0 +1,42 @@
+
+pfmpc_ds:
+  # Workspace modification
+  rho0: 0.2
+  gamma: 0.9
+  make_convex: 1
+  iterative_rho_reduction: 0
+  max_obs_compute_time: 100
+  hull_epsilon: 0.2
+  use_prev_workspace: 1
+  workspace_horizon: 0
+  velocity_obstacle: 1
+  # Target generation
+  convergence_tolerance: 0.1
+  max_rhrp_compute_time: 46
+  crep: 1.2
+  reactivity: 1.
+  buffer: 1
+  rhrp_steps: 12
+  nominal_rhrp_horizon: 2
+  # MPC
+  ce: 1
+  cs: 1.
+  R: [0.1, 0.1]
+  DR: [0.1, 0.]
+  convergence_margin: 0.02
+  lambda: 0.5
+
+  #-------- MPC Build params --------- #
+  build_mode: 'release'
+  integration_method: 'RK4'
+  n_pol: 6
+  N: 6
+  dt: 0.2
+  solver_tol: 1.e-5
+  solver_max_time: 100 # Maximum duration for mpc solver in milliseconds
+  solver_max_inner_iterations: 110
+  solver_max_outer_iterations: 10
+  solver_initial_tol: 1.e-4
+  solver_delta_tol: 1.e-4
+  solver_weight_update_factor: 10.0
+  solver_initial_penalty: 1000.0
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml
new file mode 100644
index 0000000..dd1f359
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/params/pfmpc_obstacle_constraints_params.yaml
@@ -0,0 +1,35 @@
+
+pfmpc_obstacle_constraints:
+  # Target generation
+  convergence_tolerance: 1.e-3
+  rhrp_steps: 24
+#  max_compute_time: 10
+#  buffer: 1
+  # MPC
+  ce: 1
+  cs: 1.
+  R: [0.2, 0.1]
+  DR: [0.01, 0.]
+  convergence_margin: 0.02
+
+  obs_pen: 1.e+5
+  obstacle_padding: 0.05
+
+  #-------- MPC Build params --------- #
+  build_mode: 'release'
+  integration_method: 'RK4'
+  n_pol: 6
+  N: 12
+  dt: 0.2
+  max_No_ell: 10
+  N_obs_predict: 12
+  max_No_pol: 10
+  max_No_vert: 10
+  solver_tol: 1.e-5
+  solver_max_time: 600 # Maximum duration for mpc solver in milliseconds
+  solver_max_inner_iterations: 100
+  solver_max_outer_iterations: 10
+  solver_initial_tol: 1.e-4
+  solver_delta_tol: 1.e-4
+  solver_weight_update_factor: 10.0
+  solver_initial_penalty: 1000.0
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml
new file mode 100644
index 0000000..0b6c1d5
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/params/robot_params.yaml
@@ -0,0 +1,34 @@
+
+Omnidirectional:
+  model: 'Omnidirectional'
+  radius: 0.
+  vel_max: 1.5
+
+Unicycle_forward:
+  model: 'Unicycle'
+  radius: 0.
+  lin_vel_min: 0.
+  lin_vel_max: 1.5
+  ang_vel_max: 1.5
+
+Unicycle_path_following_paper:
+  model: 'Unicycle'
+  radius: 0.
+  lin_vel_min: -0.1
+  lin_vel_max: 1
+  ang_vel_max: 1
+
+Unicycle:
+  model: 'Unicycle'
+  radius: 0.
+  lin_vel_min: -0.1
+  lin_vel_max: 1.
+  ang_vel_max: 1.5
+
+Bicycle:
+  model: 'Bicycle'
+  radius: 0.5
+  steer_max: 1.0
+  lin_vel_min: -0.5
+  lin_vel_max: 1.5
+  steer_vel_max: 0.5
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml b/python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml
new file mode 100644
index 0000000..d21a03d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/params/soads_ctrl_params.yaml
@@ -0,0 +1,14 @@
+soads:
+  dt: 0.1
+  starify: 1
+  hull_epsilon: 0.5
+  max_compute_time: 1000
+  make_convex: 0
+  adapt_obstacle_velocity: 1
+  unit_magnitude: 0
+  crep: 1.5
+  reactivity: 1.
+  tail_effect: 0
+  convergence_tolerance: 0.01
+  lin_vel: 1.5
+  dp_decay_rate: 0.5
diff --git a/python/ur_simple_control/path_generation/star_navigation/config/scene.py b/python/ur_simple_control/path_generation/star_navigation/config/scene.py
new file mode 100644
index 0000000..9cc21f9
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/config/scene.py
@@ -0,0 +1,769 @@
+import numpy as np
+from starworlds.obstacles import Ellipse, StarshapedPolygon, motion_model, Polygon, StarshapedPrimitiveCombination
+from starworlds.utils.misc import draw_shapely_polygon
+import matplotlib.pyplot as plt
+import shapely
+
+
+def zig_zag_motion_model(init_pos, p1, p2, vel):
+    return motion_model.Waypoints(init_pos, [p1, p2] * 10, vel=vel)
+
+
+def scene_config(id=None):
+    scene_id = 0
+    scene_description = {}
+    obstacles_to_plot = None
+    workspace = None
+    ws_attractors = None
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'No obstacle. Single attractor.'
+    if id == scene_id:
+        # workspace = StarshapedPolygon([[-5.3, -2.3], [5.3, -2.3], [5.3, 2.3], [-5.3, 2.3]])
+        obstacles = [
+        ]
+        p0 = np.array([-5., 2.])
+        reference_path = [[3, 0]]
+        theta0 = 0 * np.pi / 2
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'No obstacle. Infinity path.'
+    if id == scene_id:
+        # workspace = StarshapedPolygon([[-6.3, -2.3], [6.3, -2.3], [6., 2.3], [-6., 2.3]])
+        obstacles = [
+        ]
+        reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+                           3 * np.sin(4 * np.pi / 90 * s)]
+                          for s in range(int(1.5 * 90))]
+        p0 = np.array(reference_path[0])
+        theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+                            reference_path[1][0] - reference_path[0][0])
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Single circle obstacle. Single attractor.'
+    if id == scene_id:
+        workspace = Ellipse([6., 6.], motion_model=motion_model.Static(pos=[0., 0.]))
+        workspace = StarshapedPolygon([[-6., -6.], [6., -6.], [6., 6.], [-6., 6.]])
+        obstacles = [
+            Ellipse([2., 7.], motion_model=motion_model.Static(pos=[0., -2.]))
+        ]
+        reference_path = [[2.2, -5.2]]
+        reference_path = [[5, -4.]]
+        p0 = np.array([-5, 0])
+        theta0 = 0
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Single circle obstacle. Infinity path.'
+    if id == scene_id:
+        obstacles = [
+            Ellipse([1., 1.], motion_model=motion_model.Static(pos=[0., 0.]))
+        ]
+        reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+                           3 * np.sin(4 * np.pi / 90 * s)]
+                          for s in range(int(1.5 * 90))]
+        p0 = np.array(reference_path[0])
+        theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+                            reference_path[1][0] - reference_path[0][0])
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Cross obstacle and one circle. Infinity path.'
+    if id == scene_id:
+        obstacles = [
+            StarshapedPolygon([[-1.5, -0.2], [1.5, -0.2], [1.5, 0.2], [-1.5, 0.2]]),
+            StarshapedPolygon([[-0.2, -1.5], [-0.2, 1.5], [0.2, 1.5], [0.2, -1.5]]),
+            Ellipse([1., 1.], motion_model=motion_model.Static(pos=[4., 3.5]))
+        ]
+        reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+                           3 * np.sin(4 * np.pi / 90 * s)]
+                          for s in range(90)]
+        p0 = np.array(reference_path[0])
+        theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+                            reference_path[1][0] - reference_path[0][0])
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+        obstacles_to_plot = [StarshapedPrimitiveCombination(obstacles[:2], None, xr=[0, 0]), obstacles[-1]]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Two circle obstacles. Infinity path.'
+    if id == scene_id:
+        obstacles = [
+            Ellipse([1., 1.], motion_model=motion_model.Static(pos=[0., 0.])),
+            Ellipse([1., 1.], motion_model=motion_model.Static(pos=[4., 3.5])),
+        ]
+        reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+                           3 * np.sin(4 * np.pi / 90 * s)]
+                          for s in range(2 * 90)]
+        p0 = np.array(reference_path[0])
+        theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+                            reference_path[1][0] - reference_path[0][0])
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Static crowd. Single attractor.'
+    if id == scene_id:
+        workspace = StarshapedPolygon([[-3.5, -2], [5.5, -2], [5.5, 2], [-3.5, 2]])
+        obstacles = [
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 1.7])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([-.5, 0.9])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 0.1])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 1.6])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([4, 0.8])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([0.5, 2.8])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, 1.4])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1., -.4])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -1.2])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([3., 2.2])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -2.2]))
+        ]
+        reference_path = [[5., 1]]
+        p0 = np.array([-3, 1])
+        theta0 = 0
+        xlim = [-4, 6]
+        ylim = [-4, 6]
+
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Static crowd. Infinity path.'
+    if id == scene_id:
+        obstacles = [
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([4.3, 3.2])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.2, 1.3])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.4, 2])),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.4, 0.5])),
+
+            # Ellipse([0.7, 0.7], motion_model=motion_model.Static([4.3, 3.2])),
+            # Ellipse([0.7, 0.7], motion_model=motion_model.Static([1.2, 1.3])),
+            # Ellipse([0.7, 0.7], motion_model=motion_model.Static([1.5, 2.5])),
+            # Ellipse([0.7, 0.7], motion_model=motion_model.Static([1.5, 0.])),
+            StarshapedPolygon([[0, 0], [3, 0], [3, 1], [0, 1]], motion_model=motion_model.Static([-4, -2], rot=-np.pi/4), is_convex=True),
+            StarshapedPolygon([[0, 0], [1, 0], [1, 3], [0, 3]], motion_model=motion_model.Static([-4, -2], rot=-np.pi/4), is_convex=True),
+
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 1.7])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([-.5, 0.9])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 0.1])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 1.6])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([4, 0.8])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([0.5, 2.8])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, 1.4])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1., -.4])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -1.2])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([3., 2.2])),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Static([1.3, -2.2]))
+        ]
+        obstacles_to_plot = obstacles[:-2] + [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-2:]]))]
+        reference_path = [[6 * np.cos(2 * np.pi * th / 36.5),
+                           3 * np.sin(4 * np.pi * th / 36.5)]
+                          for th in np.linspace(0,36.5,100)]
+        p0 = np.array(reference_path[0]) #+ np.array([-2, 0])
+        theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+                            reference_path[1][0] - reference_path[0][0])
+        xlim = [-6.5, 6.5]
+        ylim = [-5, 5]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Dynamic crowd. Single attractor.'
+    if id == scene_id:
+        obstacles = [
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, -5], x1_mag=-0.2)),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[4, -3], x1_mag=-0.5)),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[7, -1], x1_mag=-0.2)),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[5, -2], x1_mag=-0.25, x2_mag=-0.2)),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[1, -5], x1_mag=0.3, x2_mag=0.5)),
+            Ellipse([0.4, 1], motion_model=motion_model.SinusVelocity(pos=[8, -3], x2_mag=2, x2_period=5)),
+            # Ellipse([2, 1], motion_model=motion_model.SinusVelocity(pos=[7, -2], x1_mag=2, x1_period=3)),
+            # Ellipse([0.4, 0.4], motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)),
+        ]
+        # reference_path = [[0., -3.], [11, -3]]
+        reference_path = [[11, -3]]
+        p0 = np.array([0., -3.])
+        theta0 = 0
+        xlim = [-1, 12]
+        ylim = [-8, 4]
+
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Static mixed rectangle and circle scene. Single attractor.'
+    if id == scene_id:
+        ell_ax = [0.8, 0.8]
+        obstacles = [
+            StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], motion_model=motion_model.Static([2, -3]), is_convex=True),
+            StarshapedPolygon([(3, 9), (3, 10), (6, 10), (6, 9)], motion_model=motion_model.Static([2, -3]), is_convex=True),
+            StarshapedPolygon([(3, 6), (3, 7), (0, 7), (0, 6)], motion_model=motion_model.Static([2, -3]), is_convex=True),
+            StarshapedPolygon([(8, 6), (9, 6), (9, 10), (8, 10)],  motion_model=motion_model.Static([-1, -6]), is_convex=True),
+            StarshapedPolygon([(0, 1.2), (2.5, 1.2), (2.5, 2), (0, 2)], is_convex=True),
+            StarshapedPolygon([(0, 0), (2.5, 0), (2.5, 0.8), (0, 0.8)], is_convex=True),
+            StarshapedPolygon([(2, 0), (2.5, 0), (2.5, 2), (2, 2)], is_convex=True),
+            Ellipse([0.5, 2], n_pol=100, motion_model=motion_model.Static([2, 9], rot=np.pi/4)),
+            Ellipse([0.5, 2], n_pol=100, motion_model=motion_model.Static([2, 7], rot=-np.pi/4)),
+            Ellipse(ell_ax, motion_model=motion_model.Static([0.5, 6.5])),
+            Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 7.5])),
+            Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])),
+        ]
+        reference_path = [[9., 5.]]
+        p0 = np.array([1, 8.])
+        theta0 = 0
+        xlim = [-1, 12]
+        ylim = [-1, 12]
+
+        workspace = StarshapedPolygon([[-6, -6], [6, -6], [6, 6], [-6, 6]])
+        obstacles = [
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([5, -1.5])),
+            StarshapedPolygon([(0, 0), (1, 0), (1, 5), (0, 5)], motion_model=motion_model.Static([-2, -1]),
+                              is_convex=True),
+            Ellipse([0.5, 1.5], motion_model=motion_model.Static([-3, 3], rot=-np.pi/4)),
+            Ellipse([0.5, 1.5], motion_model=motion_model.Static([-3, 0], rot=np.pi/4)),
+            StarshapedPolygon([(0, 0), (1, 0), (1, 4), (0, 4)], motion_model=motion_model.Static([1, -6]),
+                              is_convex=True),
+            StarshapedPolygon([(0, 3), (4, 3), (4, 4), (0, 4)], motion_model=motion_model.Static([1, -6]),
+                              is_convex=True),
+            StarshapedPolygon([(0, 0), (1, 0), (1, 4), (0, 4)], motion_model=motion_model.Static([0, 1]),
+                              is_convex=True),
+            StarshapedPolygon([(3, 0), (4, 0), (4, 4), (3, 4)], motion_model=motion_model.Static([0, 1]),
+                              is_convex=True),
+            StarshapedPolygon([(0, 0), (4, 0), (4, 1), (0, 1)], motion_model=motion_model.Static([0, 1]),
+                              is_convex=True),
+            StarshapedPolygon([(1.5, 1), (1.5, -2), (2.5, -2), (2.5, 1)], motion_model=motion_model.Static([0, 1]),
+                              is_convex=True),
+            StarshapedPolygon([(1.5, -2), (4, -2), (4, -1), (1.5, -1)], motion_model=motion_model.Static([0, 1]),
+                              is_convex=True),
+            StarshapedPolygon([(0, 1.4), (3, 1.4), (3, 2.4), (0, 2.4)], motion_model=motion_model.Static([-4, -5]),
+                              is_convex=True),
+            StarshapedPolygon([(0, 0), (3, 0), (3, 1), (0, 1)], motion_model=motion_model.Static([-4, -5]),
+                              is_convex=True),
+            StarshapedPolygon([(0, 0), (1, 0), (1, 1.5), (0, 1.5)], motion_model=motion_model.Static([-2, -5]),
+                              is_convex=True),
+        ]
+        xlim = [-7, 7]
+        ylim = [-7, 7]
+        reference_path = [[0., 0.]]
+        p0 = np.array([-2.5, 1.5])
+        # p0 = np.array([2.4, -5.6])
+        p0 = np.array([3, .5])
+        p0 = np.array([2, 2.5])
+        # p0 = np.array([-5, 1.5])
+        # p0 = np.array([-2.8, -3.8])
+        obstacles_to_plot = obstacles[:4] + [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[4:6]])),
+                                             Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[6:11]])),
+                                             Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[11:]]))]
+
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Static mixed rectangle and circle scene. Given path.'
+    if id == scene_id:
+        ell_ax = [0.5, 0.5]
+        obstacles = [
+            StarshapedPolygon([(7, 5), (6, 5), (6, 3), (7, 3)], is_convex=True),
+            StarshapedPolygon([(6, 3), (6, 7), (5, 7), (5, 3)], is_convex=True),
+            StarshapedPolygon([(7, 1), (8, 1), (8, 5), (7, 5)], is_convex=True),
+            StarshapedPolygon([(6, 7), (6, 8), (4, 8), (4, 7)], is_convex=True),
+            StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], is_convex=True),
+            StarshapedPolygon([(3, 9), (3, 10), (6, 10), (6, 9)], is_convex=True),
+            StarshapedPolygon([(8, 6), (9, 6), (9, 10), (8, 10)], is_convex=True),
+            Ellipse(ell_ax, motion_model=motion_model.Static([7.5, 8])),
+            Ellipse(ell_ax, motion_model=motion_model.Static([4.8, 5])),
+            Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])),
+            Ellipse(ell_ax, motion_model=motion_model.Static([3, 8.5])),
+        ]
+        reference_path = [[1., 9.], [1., 5.5], [3.5, 5.5], [3.5, 8.5], [7., 8.5], [7., 5.5], [9., 5.5], [9., 4]]
+        p0 = np.array(reference_path[0])
+        theta0 = -np.pi / 2
+        xlim = [-1, 11]
+        ylim = [-1, 11]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Static DSW equivalent scene. Single attractor.'
+    if id == scene_id:
+        workspace = StarshapedPolygon(shapely.ops.unary_union([
+            shapely.geometry.Polygon([[-5, -6], [8, -6], [8, 1], [6, 1], [6, 6], [-5, 6], [-5, 0], [-3, -4]]),
+            shapely.geometry.Point([2.5, -6]).buffer(2)]))
+        # workspace = StarshapedPolygon([[-5, -6], [8, -6], [8, 1], [6, 1], [6, 6], [-5, 6], [-5, 0], [-3, -4]])
+        # workspace = StarshapedPolygon([[-5, -8], [10, -8], [10, 6], [-5, 6]])
+
+        obstacles = [
+            StarshapedPolygon([[0, 0], [3, 0], [3, 1], [0, 1]], motion_model=motion_model.Static([-3, 1], np.pi/4), is_convex=True),
+            StarshapedPolygon([[3, 0], [3, 3], [2, 3], [2, 0]], motion_model=motion_model.Static([-3, 1], np.pi/4), is_convex=True),
+            StarshapedPolygon([[-4, 0], [3, 0], [3, 1], [-4, 1]], motion_model=motion_model.Static([3, -5]), is_convex=True),
+            StarshapedPolygon([[0, 0], [0, 4], [-1, 4], [-1, 0]], motion_model=motion_model.Static([3, -5]), is_convex=True),
+            StarshapedPolygon([[0, 0], [4, 0], [4, 1], [0, 1]], motion_model=motion_model.Static([-1.5, 0]), is_convex=True),
+            StarshapedPolygon([[0, 0], [1, 0], [1, 4], [0, 4]], motion_model=motion_model.Static([0, -1.5]), is_convex=True),
+            StarshapedPolygon([[-.5, 0], [2.5, 0], [2.5, 1], [-.5, 1]], motion_model=motion_model.Static([6, -3.5]), is_convex=True),
+            StarshapedPolygon([[1.5, -2], [2.5, -2], [2.5, 3], [1.5, 3]], motion_model=motion_model.Static([6, -3.5]), is_convex=True),
+            Ellipse([0.5, 0.5], motion_model=motion_model.Static([-1, 2.5])),
+            Ellipse([0.8, 1.3], motion_model=motion_model.Static([4, 2], np.pi / 10)),
+            Ellipse([0.8, 0.8], motion_model=motion_model.Static([3, 2.5])),
+            Ellipse([1, 1], motion_model=motion_model.Static([1.5, -3.5])),
+            Ellipse([0.8, 0.8], motion_model=motion_model.Static([-1.2, -1.8])),
+            Ellipse([1, 1], motion_model=motion_model.Static([5, -1])),
+            Ellipse([0.7, 0.7], motion_model=motion_model.Static([-2.8, -5.5])),
+        ]
+        reference_path = [[2.5, -7]]
+        p0 = np.array([-3., 3])
+        theta0 = 0
+        xlim = [-6, 9]
+        ylim = [-8, 7]
+        obstacles_to_plot = [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[:2]])),
+                             StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[2:4]])),
+                             StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[4:6]])),
+                             StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[6:8]]))]\
+                            + obstacles[8:]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Dynamic crowd. Single attractor.'
+    if id == scene_id:
+        mean_vel = 0.2
+        obstacles = [
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[3, 5], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 4], x2_mag=-(mean_vel+0.1))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 9], x2_mag=-(mean_vel+0.3))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0, 6], x2_mag=-(mean_vel+0.1))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-4, 4], x2_mag=-(mean_vel+0.3))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 2], x2_mag=-(mean_vel+0.1))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.5, 3], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.5, 3], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.3, -1], x2_mag=-(mean_vel+0.3))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0, 0], x2_mag=-(mean_vel+0.1))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 4.5], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 7], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 10], x2_mag=-(mean_vel+0.1))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1, 11], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0.5, 2], x2_mag=-(mean_vel+0.4))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-0.5, 3], x2_mag=-(mean_vel+0.3))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1, 5], x2_mag=-(mean_vel+0.1))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-2.4, 7], x2_mag=-(mean_vel+0.2))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1.5, 8], x2_mag=-(mean_vel+0.3))),
+            Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-3, 5], x2_mag=-(mean_vel+0.1))),
+        ]
+        # reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+        #                    3 * np.sin(4 * np.pi / 90 * s)]
+        #                   for s in range(8 * 90)]
+        # p0 = np.array(reference_path[0])
+        # theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+        #                     reference_path[1][0] - reference_path[0][0])
+        reference_path = [[0, 10.]]
+        p0 = np.array([-4., -5.])
+        # reference_path = [p0.tolist()] + reference_path
+        theta0 = np.pi / 2
+        xlim = [-8, 8]
+        ylim = [-6, 12]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Corridor.'
+    if id == scene_id:
+        obstacles = [
+            # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+            StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]),
+            StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+            StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]),
+            Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])),
+            # StarshapedPolygon(Ellipse([1, 1]).polygon(), motion_model=motion_model.Interval([-1, 4], [(9, (10, 4))])),
+            # Ellipse([1, 1], motion_model=motion_model.Interval([-2, 4], [(9, (11, 4))])),
+        ]
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        reference_path = [[0.5, 5.5]]
+        theta0 = np.pi
+        p0 = np.array([9, 4])
+        xlim = [0, 10]
+        ylim = [0, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Dynamic crowd and static polygons. Infinity path.'
+    if id == scene_id:
+
+        obstacles = [
+            Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([4, 3], [4, 4], [4, -4], 0.2)),
+            Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([3, 0], [3, 3], [3, -3], 0.2)),
+            Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([-3, -3], [-3, -4], [-3, 4], 0.2)),
+            Ellipse([0.5, 0.5], motion_model=zig_zag_motion_model([2.5, 0], [4, 0], [-4, 0], 0.2)),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.Waypoints(*zig_zag([4, 3], [4, 3], [4, -3]), vel=0.2)),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 0], x2_mag=-.1, x2_period=30)),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-1, 0], x2_mag=.1, x2_period=20)),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[2, 0], x2_mag=-.3, x2_period=40)),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[-2, 0], x2_mag=.2, x2_period=30)),
+            # Ellipse([0.5, 0.5], motion_model=motion_model.SinusVelocity(pos=[0, 0], x2_mag=.3, x2_period=25)),
+            # StarshapedPolygon([[-3, -2], [-1, -2], [-1, -1.5], [-2.5, -1.5], [-2.5, 0], [-3, 0]])
+            StarshapedPolygon([[-3, -2], [-1, -2], [-1, -1.5], [-3, -1.5]]),
+            StarshapedPolygon([[-3, -2], [-2.5, -2], [-2.5, 0], [-3, 0]]),
+        ]
+        reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+                           3 * np.sin(4 * np.pi / 90 * s)]
+                          for s in range(3 * 90)]
+        p0 = np.array(reference_path[0])
+        theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+                            reference_path[1][0] - reference_path[0][0])
+        xlim = [-10, 10]
+        ylim = [-10, 10]
+        obstacles_to_plot = obstacles[:-2] + [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-2:]]))]
+
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Ellipse trap. Single attractor.'
+    if id == scene_id:
+        obstacles = [
+            Ellipse([1, .5], motion_model=motion_model.Static([5, 5], np.pi/2)),
+            Ellipse([2, .5], motion_model=motion_model.Static([3, 6], 0)),
+            Ellipse([2, .5], motion_model=motion_model.Static([3, 4], 0)),
+            # StarshapedPolygon(Ellipse([1, 1]).polygon(), motion_model=motion_model.Interval([-1, 4], [(9, (10, 4))])),
+            # Ellipse([1, 1], motion_model=motion_model.Interval([-2, 4], [(9, (11, 4))])),
+        ]
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        reference_path = [[9, 5]]
+        theta0 = 0
+        p0 = np.array([4, 5])
+        xlim = [0, 10]
+        ylim = [0, 10]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Several boundaries. Waypoints.'
+    if id == scene_id:
+        workspace = [
+                     StarshapedPolygon([[-6, 3], [6, 3], [6, 6], [-6, 6]]),
+                     StarshapedPolygon([[-6, -8], [-3, -8], [-3, 6], [-6, 6]]),
+                     StarshapedPolygon([[-6, -8], [6, -8], [6, -5], [-6, -5]]),
+                     StarshapedPolygon([[3, -8], [6, -8], [6, 0], [3, 0]]),
+                     ]
+        ws_attractors = [
+            [-4.5, 4.5], #[[-6., 3], [-3, 3]],
+            [-3, -6],
+            [5, -6], [5, -2]]
+        ws_attractors = [[-3.5, 4.5], [-3.5, -6], [5, -6], [5, -2]]
+        obstacles = [
+            # Ellipse([0.7, 0.7], motion_model=zig_zag_motion_model([-3.5, -2], [-3.5, -4], [-3.5, 1], 0.2)),
+            Ellipse([0.7, 0.7], motion_model=motion_model.Waypoints([-3.5, -2], [[-3.5, -4], [-3.5, -0.5]], vel=0.2)),
+            StarshapedPolygon([[-5, -2], [-4, -2], [-4, 1], [-5, 1]]),
+            Ellipse([1, 1], motion_model=zig_zag_motion_model([0, -6], [-4, -6], [1, -6], 0.2)),
+            Ellipse([1, 1], motion_model=zig_zag_motion_model([-4, 3.6], [4, 3.6], [-4, 3.6], 0.2)),
+            Ellipse([.5, .5], motion_model=zig_zag_motion_model([-0.5, 4.5], [-4, 4.5], [3, 4.5], 0.2)),
+            # Ellipse([.5, .5], motion_model=zig_zag_motion_model([3, -6], [-3, -10], [3, -6], 0.2)),
+            # StarshapedPolygon([[-3, -2], [-2, -2], [-2, 1], [-3, 1]]),
+        ]
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        theta0 = np.pi
+        p0 = np.array([5, 4.5])
+        reference_path = [ws_attractors[0]]
+        xlim = [-8, 8]
+        ylim = [-9, 7]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Several boundaries. Path.'
+    if id == scene_id:
+        workspace = [
+                     StarshapedPolygon([[-6, 3], [6, 3], [6, 6], [-6, 6]]),
+                     StarshapedPolygon([[-6, -8], [-3, -8], [-3, 6], [-6, 6]]),
+                     StarshapedPolygon([[-6, -8], [6, -8], [6, -5], [-6, -5]]),
+                     StarshapedPolygon([[3, -8], [6, -8], [6, 0], [3, 0]]),
+                     ]
+        ws_attractors = [
+            [-4.5, 4.5], #[[-6., 3], [-3, 3]],
+            [-3, -6],
+            [5, -6], [5, -2]]
+        ws_attractors = [[-3.5, 4.5], [-3.5, -6], [5, -6], [5, -2]]
+        obstacles = [
+            # Ellipse([0.7, 0.7], motion_model=zig_zag_motion_model([-3.5, -2], [-3.5, -4], [-3.5, 1], 0.2)),
+            Ellipse([0.7, 0.7], motion_model=motion_model.Waypoints([-3.5, -2], [[-3.5, -4], [-3.5, -0.5]], vel=0.2)),
+            StarshapedPolygon([[-5, -2], [-4, -2], [-4, 1], [-5, 1]]),
+            Ellipse([1, 1], motion_model=zig_zag_motion_model([0, -6], [-4, -6], [1, -6], 0.2)),
+            Ellipse([1, 1], motion_model=zig_zag_motion_model([-4, 3.6], [4, 3.6], [-4, 3.6], 0.2)),
+            Ellipse([.5, .5], motion_model=zig_zag_motion_model([-0.5, 4.5], [-4, 4.5], [3, 4.5], 0.2)),
+            # Ellipse([.5, .5], motion_model=zig_zag_motion_model([3, -6], [-3, -10], [3, -6], 0.2)),
+            # StarshapedPolygon([[-3, -2], [-2, -2], [-2, 1], [-3, 1]]),
+        ]
+        # obstacles_to_plot = obstacles[:-1]
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        theta0 = np.pi
+        p0 = np.array([5, 4.5])
+        reference_path = [p0.tolist()] + ws_attractors
+        xlim = [-8, 8]
+        ylim = [-9, 7]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Enclosed corridor.'
+    if id == scene_id:
+        workspace = StarshapedPolygon([[1, 3], [9, 3], [9, 8], [1, 8]])
+        obstacles = [
+            # StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]),
+            # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+            # StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]),
+            # Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])),
+            StarshapedPolygon([[3, 4], [8, 4], [8, 5.5], [3, 5.5]]),
+            Ellipse([0.5, 0.5], n_pol=80, motion_model=motion_model.Interval([2, 3.5], [(20, (10, 3.5))])),
+            Ellipse([0.5, 0.5], n_pol=80, motion_model=zig_zag_motion_model([6, 6.6], [6, 7.4], [6, 6.], 0.3)),
+        ]
+
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        reference_path = [[2, 5.5]]
+        reference_path = [[2.5, 5]]
+        theta0 = np.pi / 1.2
+        p0 = np.array([7, 3.4])
+        xlim = [0.5, 9.5]
+        ylim = [2.5, 8.5]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'DSW example.'
+    if id == scene_id:
+        workspace = StarshapedPolygon([[0, 0], [17, 0], [17, 8], [11, 8], [11, 12], [5, 12], [5, 8], [0, 8]])
+        obstacles = [
+            # StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]),
+            # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+            # StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]),
+            # Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])),
+            StarshapedPolygon([[7, 9], [12, 9], [12, 10], [7, 10]]),
+            StarshapedPolygon([[13, 3], [14, 3], [14, 7], [11, 7], [11, 6], [13, 6]]),
+            StarshapedPolygon([[4, -1], [8, -1], [8, 2], [6.5, 2], [6.5, 0.5], [4, 0.5]]),
+            StarshapedPolygon([[12, -1], [15, -1], [15, 1], [12, 1]]),
+            Ellipse([0.5, 1], motion_model=motion_model.Static([1.5, 2])),
+            Ellipse([1, 1], motion_model=motion_model.Static([7, 7])),
+            Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 1.5], np.pi/4)),
+            Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 2.5], -np.pi/4)),
+            StarshapedPolygon([[3, 2.5], [4, 2.5], [4, 6.5], [3, 6.5]]),
+            StarshapedPolygon([[3, 4], [9, 4], [9, 5], [3, 5]]),
+        ]
+        obstacles_to_plot = (obstacles[:-4] +
+                             [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-4:-2]]))] +
+                             [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-2:]]))])
+
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        reference_path = [[2, 5.5]]
+        reference_path = [[9, 11]]
+        theta0 = np.pi/2
+        p0 = np.array([16, 2])
+        # p0 = np.array([9, 8])
+        xlim = [-1.5, 17.5]
+        ylim = [-1.5, 12.5]
+
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Illustration of ModEnv changing obstacles.'
+    if id == scene_id:
+        obstacles = [
+            StarshapedPolygon([[0, 0], [3, 0], [3, 3], [2, 3], [2, 1], [0, 1]]),
+            StarshapedPolygon([[0, 0], [1, 0], [1, 3], [0, 3]]),
+            Ellipse([0.5, 1], motion_model=motion_model.Static([3, -.5], 0 * np.pi / 2)),
+            Ellipse([.7, .7], motion_model=motion_model.Static([0, 3.2]))
+        ]
+        obstacles_to_plot = [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[:2]]))] + obstacles[2:]
+        reference_path = [[1.5, 1.6]]
+        theta0 = 0
+        p0 = np.array([0, -1])
+        xlim = [-1.5, 4]
+        ylim = [-2, 4.5]
+
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'Dynamic crowd 2. Single attractor.'
+    if id == scene_id:
+        mean_vel = 0.3
+        obstacles = [
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[3, 5], x2_mag=-(mean_vel+0.2))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[2, 4], x2_mag=-(mean_vel+0.1))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[2, 9], x2_mag=-(mean_vel+0.3))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0, 6], x2_mag=-(mean_vel+0.1))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-4, 4], x2_mag=-(mean_vel+0.3))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0, 2], x2_mag=-(mean_vel+0.1))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.5, 3], x2_mag=-(mean_vel+0.2))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.5, 10], x2_mag=-(mean_vel+0.2))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.3, -1], x2_mag=-(mean_vel+0.3))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[2, 0], x2_mag=-(mean_vel+0.1))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0.5, 9], x2_mag=-(mean_vel+0.1))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-3, 7], x2_mag=-(mean_vel+0.2))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-0.5, 12], x2_mag=-(mean_vel+0.1))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-4, 12], x2_mag=-(mean_vel+0.2))),
+            Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[3, 13], x2_mag=-(mean_vel+0.1))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1, 11], x2_mag=-(mean_vel+0.2))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[0.5, 2], x2_mag=-(mean_vel+0.4))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-0.5, 3], x2_mag=-(mean_vel+0.3))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1, 5], x2_mag=-(mean_vel+0.1))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-2.4, 7], x2_mag=-(mean_vel+0.2))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-1.5, 8], x2_mag=-(mean_vel+0.3))),
+            # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[-3, 5], x2_mag=-(mean_vel+0.1))),
+        ]
+        # reference_path = [[6 * np.cos(2 * np.pi / 90 * s),
+        #                    3 * np.sin(4 * np.pi / 90 * s)]
+        #                   for s in range(8 * 90)]
+        # p0 = np.array(reference_path[0])
+        # theta0 = np.arctan2(reference_path[1][1] - reference_path[0][1],
+        #                     reference_path[1][0] - reference_path[0][0])
+        reference_path = [[0, 10.]]
+        p0 = np.array([-4., -5.])
+        # reference_path = [p0.tolist()] + reference_path
+        theta0 = np.pi / 2
+        xlim = [-8, 8]
+        ylim = [-6, 12]
+
+    # ----------------------------------- #
+    scene_id += 1
+    scene_description[scene_id] = 'MPC steps illustration.'
+    if id == scene_id:
+        workspace = StarshapedPolygon([[0, 0], [17, 0], [17, 8], [11, 8], [11, 12], [5, 12], [5, 8], [0, 8]])
+        obstacles = [
+            # StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]),
+            # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+            # StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]),
+            # Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])),
+            # StarshapedPolygon([[7, 9], [12, 9], [12, 10], [7, 10]]),
+            # StarshapedPolygon([[4, -1], [8, -1], [8, 2], [6.5, 2], [6.5, 0.5], [4, 0.5]]),
+            # StarshapedPolygon([[12, -1], [15, -1], [15, 1], [12, 1]]),
+            Ellipse([0.5, 1], motion_model=motion_model.Static([1.5, 2])),
+            Ellipse([1, 1], motion_model=motion_model.Static([7, 7])),
+            Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 1.5], np.pi / 4)),
+            Ellipse([0.7, 1.2], motion_model=motion_model.Static([10, 2.5], -np.pi / 4)),
+            StarshapedPolygon([[3, 2.5], [4, 2.5], [4, 6.5], [3, 6.5]]),
+            StarshapedPolygon([[3, 4], [9, 4], [9, 5], [3, 5]]),
+            StarshapedPolygon([[13, 3], [14, 3], [14, 7], [13, 7]]),
+            StarshapedPolygon([[13, 7], [16, 7], [16, 6], [13, 6]]),
+            StarshapedPolygon([[13, 4], [16, 4], [16, 3], [13, 3]])
+        ]
+        obstacles_to_plot = (obstacles[:-5] +
+                             [StarshapedPolygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-5:-3]]))] +
+                             [Polygon(shapely.ops.unary_union([o.polygon() for o in obstacles[-3:]]))])
+
+        # reference_path = [[9, 4.5], [0.5, 4.5], [0.5, 5.5]]
+        # self.p0 = np.array(self.reference_path[0])
+        reference_path = [[2, 5.5]]
+        reference_path = [[9, 11]]
+        theta0 = np.pi / 2 * 0.2
+        p0 = np.array([14.5, 5])
+        # p0 = np.array([9, 8])
+        xlim = [-1.5, 17.5]
+        ylim = [-1.5, 12.5]
+
+    while id is None or not (1 <= id <= scene_id):
+        print("Select scene ID\n -------------")
+        for i, description in scene_description.items():
+            print(str(i) + ": " + description)
+        try:
+            id = int(input("-------------\nScene ID: "))
+        except:
+            pass
+        # if not valid_id(id):
+        #     print("Invalid scene id: " + str(id) + ". Valid range [1, " + str(scene_id) + "].\n")
+
+        return scene_config(id)
+
+    if obstacles_to_plot is None:
+        obstacles_to_plot = obstacles
+    return p0, theta0, reference_path, ws_attractors, obstacles, workspace, obstacles_to_plot, xlim, ylim, id
+
+
+#
+# def scene_description():
+#     return scene_config()
+
+class Scene:
+
+    def __init__(self, id=None, robot_radius=0.):
+        self.id = id
+        self.p0, self.theta0, self.reference_path, self.ws_attractors, self.obstacles, self.workspace, \
+        self._obstacles_to_plot, self.xlim, self.ylim, self.id = scene_config(id)
+        # Compute all polygon
+        [o.polygon() for o in self.obstacles]
+        [o.kernel() for o in self.obstacles]
+        [o.is_convex() for o in self.obstacles]
+        self.robot_radius = robot_radius
+        # if robot_radius > 0:
+        #     self.obstacles = [o.dilated_obstacle(padding=robot_radius, id="duplicate") for o in self.obstacles]
+        self._static_obstacle = [o._motion_model is None or o._motion_model.__class__.__name__ == "Static" for o in self.obstacles]
+        self.is_static = all(self._static_obstacle)
+
+    def step(self, dt, robot_pos):
+        if self.is_static:
+            return
+        for i, o in enumerate(self.obstacles):
+            if self._static_obstacle[i]:
+                continue
+            prev_pos, prev_rot = o.pos().copy(), o.rot()
+            o.move(dt)
+            # if not o.exterior_point(robot_pos):
+            # NOTE: Not well implemented. E.g. with hardcoded dt
+            if o.polygon().distance(shapely.geometry.Point(robot_pos)) < max(abs(o._motion_model.lin_vel()))*0.2:
+                print("[Scene]: Obstacle " + str(o) + " stands still to avoid moving into robot.")
+                o._motion_model.set_pos(prev_pos)
+                o._motion_model.set_rot(prev_rot)
+        for i in range(len(self.obstacles)):
+            if self._static_obstacle[i]:
+                continue
+            if self.obstacles[i]._motion_model is not None:
+                self._obstacles_to_plot[i]._motion_model.set_pos(self.obstacles[i].pos())
+                self._obstacles_to_plot[i]._motion_model.set_rot(self.obstacles[i].rot())
+
+    def init_plot(self, ax=None, workspace_color='k', workspace_alpha=0.6, obstacle_color='lightgrey', obstacle_edge_color='k', obstacle_alpha=1, show_obs_name=False, draw_p0=1,
+                  draw_ref=1, reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14):
+        if ax is None:
+            _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+        ax.set_xlim(self.xlim)
+        ax.set_ylim(self.ylim)
+        ax.set_aspect('equal')
+        line_handles = []
+        if draw_p0:
+            ax.plot(*self.p0, 'ks', markersize=10)
+        if draw_ref:
+            if len(self.reference_path) > 1:
+                ax.plot([r[0] for r in self.reference_path], [r[1] for r in self.reference_path], color=reference_color,
+                             alpha=reference_alpha, zorder=-2)
+            else:
+                if isinstance(self.workspace, list):
+                    [ax.plot(*attr, color=reference_color, alpha=reference_alpha,
+                            marker=reference_marker,
+                            markersize=reference_markersize) for attr in self.ws_attractors]
+                else:
+                    ax.plot(*self.reference_path[-1], color=reference_color, alpha=reference_alpha, marker=reference_marker,
+                                 markersize=reference_markersize)
+
+        if self.workspace is not None:
+            draw_shapely_polygon(pol=shapely.geometry.box(self.xlim[0], self.ylim[0], self.xlim[1], self.ylim[1]), ax=ax, fc=obstacle_color, alpha=obstacle_alpha, ec='None', zorder=-10)
+            if isinstance(self.workspace, list):
+                [b.draw(ax=ax, fc='w', ec='None', show_reference=False, zorder=-9) for b in self.workspace]
+            else:
+                self.workspace.draw(ax=ax, fc='w', ec=obstacle_edge_color, show_reference=False, zorder=-9)
+
+        for o in self._obstacles_to_plot:
+            lh, _ = o.init_plot(ax=ax, fc=obstacle_color, alpha=obstacle_alpha, ec=obstacle_edge_color, show_name=show_obs_name, show_reference=False)
+            line_handles.append(lh)
+
+        return line_handles, ax
+
+    def update_plot(self, line_handles):
+        for i, o in enumerate(self._obstacles_to_plot):
+            o.update_plot(line_handles[i])
+
+    def draw(self, ax=None, workspace_color='k', workspace_alpha=0.6, obstacle_color='lightgrey', obstacle_edge_color='k', obstacle_alpha=1, show_obs_name=False, draw_p0=1,
+                  draw_ref=1, reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14):
+        lh, ax = self.init_plot(ax, workspace_color, workspace_alpha, obstacle_color, obstacle_edge_color, obstacle_alpha, show_obs_name, draw_p0,
+                  draw_ref, reference_color, reference_alpha, reference_marker, reference_markersize)
+        self.update_plot(lh)
+        return lh, ax
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py
new file mode 100644
index 0000000..261a8ac
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/__init__.py
@@ -0,0 +1 @@
+from .dmp import DMPController, DMP
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py
new file mode 100644
index 0000000..15a2d7a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/dmp/dmp.py
@@ -0,0 +1,175 @@
+import numpy as np
+import shapely
+from starworlds.starshaped_hull import cluster_and_starify
+from starworlds.utils.misc import tic, toc
+from motion_control.soads import f_nominal as soads_f
+
+class DMP:
+
+    def __init__(self, parameters, reference_path, path_speed):
+        self.K = parameters['K']
+        self.D = parameters['D']
+        self.alpha_s = None
+        self.n_bfs = parameters['n_bfs']
+        self.n = 2 # Hardcoded 2D path
+        self.goal = np.array(reference_path.coords[-1])
+        self.p0 = np.array(reference_path.coords[0])
+        self.centers = None
+        self.widths = None
+        self.w = None
+        self.fit(reference_path, path_speed)
+        self.z0 = np.concatenate((self.p0, 0 * self.p0))
+
+    def p(self, z):
+        assert len(z) == 2*self.n
+        return z[:self.n]
+
+    def v(self, z, obstacles=None, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False,
+      convergence_tolerance=1e-4):
+        assert len(z) == 2*self.n
+        if obstacles is None and workspace is None:
+            return z[self.n:]
+        else:
+            return soads_f(z[:self.n], z[self.n:], obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect,
+      convergence_tolerance)
+
+    def f(self, z, t, obstacles=None, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False,
+      convergence_tolerance=1e-4):
+        s = np.exp(-self.alpha_s * t)
+        psi = self.basis_fcn(s)
+        fe = self.w.dot(psi) / np.maximum(np.sum(psi), 1e-8) * (self.goal - self.p0) * s
+        return self.K * (self.goal - self.p(z)) - self.D * self.v(z, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect,
+      convergence_tolerance) + fe
+
+    def dz(self, z, t, obstacles=None, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False,
+      convergence_tolerance=1e-4):
+      #   return np.concatenate((self.v(z, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect,
+      # convergence_tolerance), self.f(z, t)))
+        return np.concatenate((self.v(z, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect,
+      convergence_tolerance), self.f(z, t, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect,
+      convergence_tolerance)))
+
+    def basis_fcn(self, s, i=None):
+        if i is not None:
+            return np.exp(-1 / (2 * self.widths[i] ** 2) * (s - self.centers[i]) ** 2)
+        if i is None:
+            return np.exp(-1 / (2 * self.widths ** 2) * (s - self.centers) ** 2)
+
+    def fit(self, reference_path, path_speed):
+        delta_p = int(reference_path.length / path_speed)
+        t_demo = np.linspace(0, reference_path.length, 10000)
+        self.alpha_s = 1. / t_demo[-1]
+
+        p_demo = np.array([reference_path.interpolate(s).coords[0] for s in t_demo]).T
+        
+        # Set basis functions
+        t_centers = np.linspace(0, reference_path.length, self.n_bfs, endpoint=True)
+        self.centers = np.exp(-self.alpha_s * t_centers)
+        widths = np.abs((np.diff(self.centers)))
+        self.widths = np.concatenate((widths, [widths[-1]]))
+
+        # Calculate derivatives
+        dp_demo = (p_demo[:, 1:] - p_demo[:, :-1]) / (t_demo[1:] - t_demo[:-1])
+        dp_demo = np.concatenate((dp_demo, np.zeros((self.n, 1))), axis=1)
+        ddp_demo = (dp_demo[:, 1:] - dp_demo[:, :-1]) / (t_demo[1:] - t_demo[:-1])
+        ddp_demo = np.concatenate((ddp_demo, np.zeros((self.n, 1))), axis=1)
+
+        # Compute weights
+        x_seq = np.exp(-self.alpha_s * t_demo)
+        self.w = np.zeros((self.n, self.n_bfs))
+        for i in range(self.n):
+            if abs(self.goal[i] - self.p0[i]) < 1e-5:
+                continue
+            f_gain = x_seq * (self.goal[i] - self.p0[i])
+            f_target = ddp_demo[i, :] - self.K * (self.goal[i] - p_demo[i, :]) + self.D * dp_demo[i, :]
+            for j in range(self.n_bfs):
+                psi_j = self.basis_fcn(x_seq, j)
+                num = f_gain.dot((psi_j * f_target).T)
+                den = f_gain.dot((psi_j * f_gain).T)
+                if abs(den) < 1e-14:
+                    continue
+                self.w[i, j] = num / den
+
+
+class DMPController:
+
+    def __init__(self, params, robot, reference_path, verbose=False):
+        if not robot.__class__.__name__ == 'Omnidirectional':
+            raise NotImplementedError("DMPController only implemented for Omnidirectional robots.")
+        self.params = params
+        self.robot = robot
+        self.obstacle_clusters = None
+        self.obstacles_star = []
+        self.dp_prev = None
+        self.verbose = verbose
+        self.reference_path = shapely.geometry.LineString(reference_path)
+        self.theta_g = self.reference_path.length
+        self.dmp = DMP(params, self.reference_path, robot.vmax)
+        self.z = self.dmp.z0.copy()
+        self.u = None
+        self.theta = 0
+        self.timing = {'workspace': 0, 'target': 0, 'mpc': 0}
+
+    def compute_u(self, x):
+        return self.v_ref #+ 1 * (self.p_ref - self.robot.h(x))
+
+    def update_policy(self, x, obstacles, workspace=None):
+        p = self.robot.h(x)
+
+        # if self.params['starify']:
+        #     self.obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles, p, pg,
+        #                                                                                      self.params['hull_epsilon'],
+        #                                                                                      max_compute_time=  self.params['max_compute_time'],
+        #                                                                                      workspace=workspace,
+        #                                                                                      previous_clusters=self.obstacle_clusters,
+        #                                                                                      # dx_prev=self.dp_prev,
+        #                                                                                      make_convex=self.params['make_convex'],
+        #                                                                                      verbose=self.verbose)
+        #     self.timing['obstacle'] = sum(obstacle_timing)
+        #     self.obstacles_star = [cl.cluster_obstacle for cl in self.obstacle_clusters]
+        # else:
+        #     self.timing['obstacle'] += 0
+        #     self.obstacles_star = obstacles
+
+        # t0 = tic()
+        # dist_to_goal = np.linalg.norm(p - pg)
+
+        dtheta_dt = 0.2
+
+        # obstacles=None
+        self.v_ref = self.dmp.v(self.z, obstacles, workspace, unit_magnitude=False, crep=self.params['crep'],
+                     reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'],
+                     adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'],
+                     convergence_tolerance=self.params['convergence_tolerance']) * dtheta_dt
+        self.p_ref = self.dmp.p(self.z)
+
+        dv = (self.v_ref - self.z[2:]) / self.params['dt']
+
+        dz = np.concatenate((self.v_ref, dv))
+        dz = self.dmp.dz(self.z, self.theta, obstacles, workspace, unit_magnitude=False, crep=self.params['crep'],
+                     reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'],
+                     adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'],
+                     convergence_tolerance=self.params['convergence_tolerance'])
+
+        # Target state integration
+        # dz = np.concatenate((self.dmp.v(self.z), self.dmp.f(self.z, self.theta)))
+        self.z += dz * dtheta_dt * self.params['dt']
+
+        self.theta += dtheta_dt * self.params['dt']
+
+
+
+        # p_next = p + dp * self.params['dt']
+        #
+        # while not all([o.exterior_point(p_next) for o in self.obstacles_star]) and (workspace is None or workspace.interior_point(p_next)):
+        #     dp *= self.params['dp_decay_rate']
+        #     p_next = p + dp * self.params['dt']
+        #     # Additional compute time check
+        #     if toc(t0) > self.params['max_compute_time']:
+        #         if self.verbose or True:
+        #             print("[Max compute time in soads when adjusting for collision. ")
+        #         dp *= 0
+        #         break
+        # self.timing['control'] = toc(t0)
+        #
+        # self.dp_prev = dp
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py
new file mode 100644
index 0000000..987aeaa
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/__init__.py
@@ -0,0 +1,2 @@
+from .mpc import Mpc, pol2pos
+from .motion_controller import MotionController
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py
new file mode 100644
index 0000000..f3b6efc
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/motion_controller.py
@@ -0,0 +1,142 @@
+from motion_control.pfmpc_artificial_reference import Mpc, pol2pos
+from starworlds.utils.misc import tic, toc
+import numpy as np
+import shapely
+
+
+class MotionController:
+    def __init__(self, params, robot, reference_path, verbosity=0):
+        self.params = params
+        self.robot = robot
+        self.mpc = Mpc(params, robot)
+        self.verbosity = verbosity
+        self.reference_path = shapely.geometry.Point(reference_path[0]) if len(reference_path) == 1 else shapely.geometry.LineString(reference_path)  # Waypoints
+        self.theta_g = self.reference_path.length
+        self.rhrp_L = self.params['N'] * self.params['dt'] * self.mpc.build_params['w_max']
+        self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps'])
+        self.reset()
+
+    def reset(self):
+        self.mpc.reset()
+        self.theta = 0
+        self.rhrp_path = None
+        # self.rhrp_L = self.params['N'] * self.mpc.build_params['dp_max']
+        # self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps'])
+        # self.receding_ds = self.params['normalized_reference_stepsize'] * self.mpc.build_params['dp_max']
+        # self.rhrp_s = np.arange(0, self.rhrp_L + self.receding_ds, self.receding_ds)
+        self.path_pol = None
+        self.epsilon = None
+        self.solution = None
+        self.sol_feasible = None
+        self.u_prev = [0] * self.robot.nu
+        self.timing = {'workspace': 0, 'target': 0, 'mpc': 0}
+
+    def extract_obs_par(self, obstacles):
+        n_ell_par = 3 * self.mpc.build_params['max_No_ell'] * (1 + self.mpc.build_params['N_obs_predict'])
+        n_pol_par = self.mpc.build_params['max_No_pol'] * self.mpc.build_params['max_No_vert'] * 2
+        obs_par = [0] * (n_ell_par + n_pol_par)
+        obs_par[1:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params['max_No_ell']
+        obs_par[2:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params['max_No_ell']
+        obs_par_padded = obs_par.copy()
+        n_ell, n_pol = 0, 0
+
+        for o in obstacles:
+            if hasattr(o, "_a"):
+                # print(o.pos())
+                j = n_ell * 3 * (1 + self.mpc.build_params['N_obs_predict'])
+                obs_par[j] = 1  # Include obstacle
+                obs_par[j + 1:j + 3] = o._a # Ellipse axes
+                # Ugly coding for prediction
+                mm = o._motion_model
+                pos, rot, t = mm.pos().copy(), mm.rot(), mm._t
+                if hasattr(mm, '_wp_idx'):
+                    wp_idx = mm._wp_idx
+                for k in range(self.mpc.build_params['N_obs_predict']):
+                    obs_par[j + 3 + 3 * k:j + 5 + 3 * k] = mm.pos()  # Ellipse position
+                    obs_par[j + 5 + 3 * k] = mm.rot()  # Ellipse orientation
+                    mm.move(None, self.mpc.build_params['dt'])
+                mm.set_pos(pos), mm.set_rot(rot)
+                mm._t = t
+                if hasattr(mm, '_wp_idx'):
+                    mm._wp_idx = wp_idx
+
+                obs_par_padded[j:j+3*(self.mpc.build_params['N_obs_predict']+1)] = obs_par[j:j+3*(self.mpc.build_params['N_obs_predict']+1)]
+                obs_par_padded[j + 1:j + 3] = o._a + self.params['obstacle_padding']
+
+                n_ell += 1
+
+            if hasattr(o, "vertices"):
+                idx = n_ell_par + n_pol * self.mpc.build_params['max_No_vert'] * 2
+                vertices = shapely.ops.orient(o.polygon()).exterior.coords[:-1]
+                for i in range(self.mpc.build_params['max_No_vert']):
+                    obs_par[idx+i*2:idx+(i+1)*2] = vertices[i % len(vertices)]
+                vertices = shapely.ops.orient(o.polygon().buffer(self.params['obstacle_padding'], quad_segs=1, cap_style=3, join_style=2)).exterior.coords[:-1]
+                for i in range(self.mpc.build_params['max_No_vert']):
+                    obs_par_padded[idx+i*2:idx+(i+1)*2] = vertices[i % len(vertices)]
+                n_pol += 1
+
+        # for i in range(self.mpc.build_params['max_No_ell']):
+        #     j = i * 3 * (1 + self.mpc.build_params['N_obs_predict'])
+        #     include_obs = obs_par[j]
+        #     ell_axs = obs_par[j + 1:j + 3]
+        #     ell_pos = obs_par[j + 3 + 3 * k:j + 5 + 3 * k]
+        #     ell_rot = obs_par[j + 5 + 3 * k]
+        #     print(include_obs,ell_axs,ell_pos,ell_rot)
+        #
+        # for i in range(self.mpc.build_params['max_No_pol']):
+        #     j = n_ell_par + i * self.mpc.build_params['max_No_vert'] * 2
+        #     print(obs_par[j : j + 2 * self.mpc.build_params['max_No_vert']])
+        return obs_par, obs_par_padded
+
+    def compute_u(self, x):
+        return self.u_prev
+
+    def update_policy(self, x, obstacles, workspace=None):
+        p = self.robot.h(x)
+
+        # Extract receding path from global target path
+        t0 = tic()
+        rhrp_path_sh = shapely.ops.substring(self.reference_path, start_dist=self.theta, end_dist=self.theta + self.rhrp_L)
+        if rhrp_path_sh.length > 0:
+            self.rhrp_path = np.array([rhrp_path_sh.interpolate(s).coords[0] for s in self.rhrp_s])
+        else:
+            self.rhrp_path = np.tile(rhrp_path_sh.coords[0], (len(self.rhrp_s), 1))
+
+        # Approximate target with polynomials
+        self.path_pol = np.polyfit(self.rhrp_s, self.rhrp_path[:, 0], self.params['n_pol']).tolist() + \
+                        np.polyfit(self.rhrp_s, self.rhrp_path[:, 1], self.params['n_pol']).tolist()
+        # Force init position to be correct
+        self.path_pol[self.params['n_pol']] = self.rhrp_path[0, 0]
+        self.path_pol[-1] = self.rhrp_path[0, 1]
+        # Compute polyfit approximation error
+        self.epsilon = max(np.linalg.norm(self.rhrp_path - np.array([pol2pos(self.path_pol, s, self.mpc.build_params['n_pol']) for s in self.rhrp_s]), axis=1))
+        self.timing['target'] = toc(t0)
+
+        t0 = tic()
+        # Extract obstacle parameters (Assumes all ellipses)
+        obs_par, obs_par_padded = self.extract_obs_par(obstacles)
+        # Compute MPC solution
+        if self.solution is not None:
+            init_guess = self.solution.copy()
+            init_guess[:3] = x
+        else:
+            init_guess = None
+        solution_data = self.mpc.run(x.tolist(), self.path_pol, self.params, obs_par_padded, init_guess)
+        # solution_data = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, 1, 0.1, self.solution)
+        if solution_data is None:
+            self.sol_feasible, self.mpc_exit_status = False, "None"
+        else:
+            self.solution, self.mpc_exit_status = solution_data.solution, solution_data.exit_status
+            self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, obs_par, self.params, d=self.verbosity > 0)
+        # self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, 1, 0.1, d=self.verbosity > 0)
+
+        if self.sol_feasible:
+            xa0, u, ua, w = self.mpc.sol2xa0uuaw(self.solution)
+            self.u_prev = u[:self.robot.nu]
+            p_ref_dist = rhrp_path_sh.distance(shapely.geometry.Point(p))
+            if self.theta_g > 0 and p_ref_dist < 1:
+                self.theta = min(self.theta + w[0] * self.params['dt'], self.theta_g)
+        else:
+            self.u_prev = [0] * self.robot.nu
+
+        self.timing['mpc'] = toc(t0)
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py
new file mode 100644
index 0000000..ff960bb
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc.py
@@ -0,0 +1,489 @@
+import casadi.casadi as cs
+import opengen as og
+import numpy as np
+import os, sys
+import yaml
+
+
+class NoSolutionError(Exception):
+    '''raise this when there's no solution to the mpc problem'''
+    pass
+
+def pol_eval(pol_coeff, x, n_pol):
+    return sum([pol_coeff[i] * x ** (n_pol - i) for i in range(n_pol+1)])
+
+
+def pol2pos(pol_coeff, s, n_pol):
+    return [pol_eval(pol_coeff[:n_pol+1], s, n_pol), pol_eval(pol_coeff[n_pol+1:], s, n_pol)]
+
+# Returns >0 for interior points, <0 for exterior points, =0 for boundary points
+def ellipse_function(p, pos, rot, ax):
+    x_dist = p[0] - pos[0]
+    y_dist = p[1] - pos[1]
+    s, c = cs.sin(rot), cs.cos(rot)
+    normalized_center_dist = ((x_dist * c + y_dist * s) ** 2) / (ax[0] ** 2) + ((x_dist * s + y_dist * c) ** 2) / (ax[1] ** 2)
+    return 1 - normalized_center_dist
+
+# Returns >0 for interior points, otherwise 0
+def convex_polygon_function(p, vertices, N):
+    hp_vals = []
+    for i in range(N-1):
+        idc1, idc2 = i*2, (i+1) % N * 2
+        x1, y1 = vertices[idc1], vertices[idc1+1]
+        x2, y2 = vertices[idc2], vertices[idc2+1]
+        # x2, y2 = vertices[(i + 1) % N * 2], vertices[(i + 1) % N + 1]
+        dx = x2 - x1
+        dy = y2 - y1
+        Ai = cs.vertcat(dy, -dx)
+        bi = x1 * dy - y1 * dx
+        # print(i, [x1,y1],[x2, y2], bi-cs.dot(Ai, p))
+        hp_vals += [bi-cs.dot(Ai, p)]
+    return cs.vertcat(*hp_vals)
+
+class Mpc:
+
+    def __init__(self, params, robot):
+        self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build')
+        # Load parameters
+        self.build_params = None
+        self.robot = robot
+        self.set_build_params(params)
+        rebuild, self.build_name = self.get_build_version()
+        self.generate_sol_fun()
+        # Build if different from latest build
+        if rebuild:
+            self.build()
+        else:
+            print("Found MPC build: {}".format(self.build_name))
+
+        sys.path.insert(1, os.path.join(self.build_dir, self.build_name))
+        optimizer = __import__(self.build_name)
+        self.solver = optimizer.solver()
+        self.reset()
+
+    def reset(self):
+        self.sol_prev = None
+
+    def set_build_params(self, params):
+        self.build_params = {
+            'mode': params['build_mode'],
+            'N': params['N'],
+            'dt': params['dt'],
+            'solver_tol': params['solver_tol'],
+            'solver_max_time': params['solver_max_time'],
+            'solver_max_inner_iterations': params['solver_max_inner_iterations'],
+            'solver_max_outer_iterations': params['solver_max_outer_iterations'],
+            'x_min': self.robot.x_min,
+            'x_max': self.robot.x_max,
+            'u_min': self.robot.u_min,
+            'u_max': self.robot.u_max,
+            'nx': self.robot.nx,
+            'nu': self.robot.nu,
+            'np': 2,
+            'robot_model': self.robot.__class__.__name__,
+            'n_pol': params['n_pol'],
+            'integration_method': params['integration_method'],
+            'w_max': self.robot.vmax,
+            'max_No_ell': params['max_No_ell'],
+            'max_No_pol': params['max_No_pol'],
+            'max_No_vert': params['max_No_vert'],
+            'N_obs_predict': params['N_obs_predict'],
+            'dlp_con': params['dlp_con'],
+            'xaeN_con': params['xaeN_con'],
+        }
+
+    def get_build_version(self):
+        builds = [name for name in os.listdir(self.build_dir)
+                  if os.path.isdir(os.path.join(self.build_dir, name))]
+        for build_ver in builds:
+            if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')):
+                continue
+            with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file:
+                build_ver_params = yaml.load(file, Loader=yaml.FullLoader)
+            if self.build_params == build_ver_params:
+                return False, build_ver
+        ver = 0
+        while 'ver' + str(ver) in os.listdir(self.build_dir):
+            ver += 1
+        return True, 'ver' + str(ver)
+
+    def cs_obstacle_evaluation(self, obs_par, x, k):
+        n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict'])
+        obs_val = []
+        for i in range(self.build_params['max_No_ell']):
+            j = i * 3 * (1 + self.build_params['N_obs_predict'])
+            time_idx = min(k, self.build_params['N_obs_predict']-1)
+            include_obs = obs_par[j]
+            ell_axs = obs_par[j + 1:j + 3]
+            ell_pos = obs_par[j + 3 + 3 * time_idx:j + 5 + 3 * time_idx]
+            ell_rot = obs_par[j + 5 + 3 * time_idx]
+            ell_val = ellipse_function(self.robot.h(x), ell_pos, ell_rot, ell_axs)
+            obs_val += [cs.fmax(include_obs * ell_val, 0)]
+
+        for i in range(self.build_params['max_No_pol']):
+            j = n_ell_par + i * self.build_params['max_No_vert'] * 2
+            hp_vals = convex_polygon_function(self.robot.h(x), obs_par[j : j + 2 * self.build_params['max_No_vert']], self.build_params['max_No_vert'])
+            obs_val += [cs.fmax(cs.mmin(hp_vals),0)]
+        return cs.vertcat(*obs_val)
+
+    def is_feasible(self, sol, x0, path_pol, obs_par, params, d=False):
+        con = self.sol2con(sol, x0, path_pol, params)
+        sol_min, sol_max = self.sol_bounds()
+        con_min, con_max = self.con_bounds()
+
+        obs_val = self.sol2obsval(sol, x0, obs_par)
+
+        eps = 1.e-4
+
+        def in_range(val, min, max):
+            return ((np.array(val) >= np.array(min)) & (np.array(val) <= np.array(max))).all()
+
+        sol_ok = True
+
+        if not in_range(sol, sol_min - eps, sol_max + eps):
+            sol_ok = False
+        if not in_range(con, con_min - eps, con_max + eps):
+            sol_ok = False
+        if not in_range(obs_val, 0 - eps, 0 + eps):
+            sol_ok = False
+
+        return sol_ok
+
+        # if not in_range(u, u_min, u_max):
+        #     sol_ok = False
+        #     if d:
+        #         print("[MPC]: Bad u.")
+        #         print(u)
+        # if not in_range(w, w_min, w_max):
+        #     sol_ok = False
+        #     if d:
+        #         print("[MPC]: Bad w. Not in [{:.4f}, {:.4f}]".format(0, self.build_params['dp_max']))
+        #         print(w)
+        # if not in_range(x, x_min, x_max):
+        #     sol_ok = False
+        #     if d:
+        #         print("[MPC]: Bad x")
+        #         print(x)
+        # if not in_range(s, s_min, s_max):
+        #     sol_ok = False
+        #     if d:
+        #         print("[MPC]: Bad sN {:.4f} > {:.4f}".format(s[-1], s_max[0]))
+        # return sol_ok
+
+    def error(self, x, s, path_pol):
+        p_ref = cs.vertcat(*pol2pos(path_pol, s, self.build_params['n_pol']))
+        return cs.norm_2(p_ref - self.robot.h(x))
+
+    def base_solution(self, x0, path_pol, w0):
+        u = [0] * (self.build_params['N'] * self.build_params['nu'])
+        xa0 = x0.copy()
+        w = [0] * (self.build_params['N'])
+        w[0] = w0
+
+        if self.build_params['robot_model'] == 'Unicycle':
+            p_ref = pol2pos(path_pol, w0*self.build_params['dt'], self.build_params['n_pol'])
+            k1, k2 = 10, 2
+            # k1, k2 = 0.15, 0.3
+
+            x = x0.copy()
+            for i in range(self.build_params['N']):
+                e = np.array(x[:2]) - np.array(p_ref)
+                th = x[2]
+
+                r_th = np.arctan2(e[1], e[0]) + np.pi
+                # E Theta in (-pi, pi]
+                e_th = r_th - th
+                e_th0 = e_th
+                while e_th > np.pi:
+                    e_th -= 2 * np.pi
+                while e_th <= -np.pi:
+                    e_th += 2 * np.pi
+
+                ui = [
+                    -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)),
+                    k2 * (e_th)
+                ]
+
+                if self.robot.u_min[0] == 0 and ui[0] < 0:
+                    ui[0] = 0
+                    u0_sig = 1
+                else:
+                    u0_sig = ui[0] / self.robot.u_max[0] if ui[0] >= 0 else ui[0] / self.robot.u_min[0]
+                u1_sig = ui[1] / self.robot.u_max[1] if ui[1] >= 0 else ui[1] / self.robot.u_min[1]
+
+                sig = max(u0_sig, u1_sig, 1)
+                if sig > 1:
+                    if u0_sig > u1_sig:
+                        ui[0] = self.robot.u_max[0] if ui[0] > 0 else self.robot.u_min[0]
+                        ui[1] /= sig
+                    else:
+                        ui[0] /= sig
+                        ui[1] = self.robot.u_max[1] if ui[1] > 0 else self.robot.u_min[1]
+                u[i * self.build_params['nu']:(i+1) * self.build_params['nu']] = ui
+                # print(e, e_th, ui)
+                x, _ = self.robot.move(x, ui, self.build_params['dt'])
+
+        ua = u
+
+        return xa0 + u + ua + w
+
+    def discrete_integration(self, f, x, u):
+        dt = self.build_params['dt']
+        if self.build_params['integration_method'] == 'euler':
+            return x + f(x, u) * dt
+        if self.build_params['integration_method'] == 'RK4':
+            k1 = f(x, u)
+            k2 = f(x + dt / 2 * k1, u)
+            k3 = f(x + dt / 2 * k2, u)
+            k4 = f(x + dt * k3, u)
+            return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
+        raise NotImplemented
+
+    def generate_sol_fun(self):
+        # Initialize
+        sol_sym = cs.SX.sym('sol', self.build_params['nx'] + (2 * self.build_params['nu'] + 1) * self.build_params['N'])
+        x0_sym = cs.SX.sym('x0', self.build_params['nx'])
+        path_pol_sym = cs.SX.sym('path_pol', self.build_params['np'] * (self.build_params['n_pol'] + 1))
+
+        n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict'])
+        n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2
+        obs_par_sym = cs.SX.sym('obs_par', n_ell_par + n_pol_par)
+        # obs_par_sym = cs.SX.sym('obs_par', self.build_params['max_No_ell'] * self.build_params['N_obs_predict'] * 6 + self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2)
+        cost_params = {'Q': self.build_params['nx'], 'R': self.build_params['nu'], 'K': 1,
+                       'S': self.build_params['nu'], 'T': 1, 'mu': 1}
+        cost_params_sym = cs.SX.sym("cost_params", sum(list(cost_params.values())))
+        # Exchange parameter dimension with SX variable
+        p_idx = 0
+        for key, dim in cost_params.items():
+            cost_params[key] = cost_params_sym[p_idx:p_idx + dim]
+            p_idx += dim
+        # Initialize
+        xa0, u, ua, w = self.sol2xa0uuaw(sol_sym)
+        u_k, ua_k, w_k = u[:self.build_params['nu']], ua[:self.build_params['nu']], w[0]
+        x_k, xa_k, s_k = x0_sym, xa0, 0
+        ea_k = self.error(xa_k, s_k, path_pol_sym)
+        obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, 0)
+        obs_val_a_k = self.cs_obstacle_evaluation(obs_par_sym, xa_k, 0)
+        # lp_k = cost_params['K'] * ea_k ** 2 + cs.dot(cost_params['S'], ua_k * ua_k) + cost_params['T'] * (w_k - self.build_params['dp_max']) ** 2
+        lp_k = cost_params['K'] * ea_k ** 2 + cost_params['T'] * (w_k - self.build_params['w_max']) ** 2
+        x, s, ea, xa, obs_val, obs_val_a = x_k, s_k, ea_k, xa_k, obs_val_k, obs_val_a_k
+        dlp = []
+        # Loop over time steps
+        cs_f = lambda x,u: cs.vertcat(*self.robot.f(x,u))
+        cs_fs = lambda s,w: w
+        for k in range(self.build_params['N']):
+            # Current control variables
+            u_k = u[k * self.build_params['nu']:(k + 1) * self.build_params['nu']]
+            ua_k = ua[k * self.build_params['nu']:(k + 1) * self.build_params['nu']]
+            w_k = w[k]
+            # Integrate one step
+            x_k = self.discrete_integration(cs_f, x_k, u_k)
+            s_k = self.discrete_integration(cs_fs, s_k, w_k)
+            xa_k = self.discrete_integration(cs_f, xa_k, ua_k)
+            lp_prev_k = lp_k
+            # Extract error and obstacle values
+            ea_k = self.error(xa_k, s_k, path_pol_sym)
+            obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, k+1)
+            obs_val_a_k = self.cs_obstacle_evaluation(obs_par_sym, xa_k, k+1)
+
+            # lp_k = cost_params['K'] * ea_k ** 2 + cs.dot(cost_params['S'], ua_k * ua_k) + cost_params['T'] * (w_k - self.build_params['dp_max']) ** 2
+            lp_k = cost_params['K'] * ea_k ** 2 + cost_params['T'] * (w_k - self.build_params['w_max']) ** 2
+            dlp_k = lp_k - lp_prev_k
+
+            # Store current state
+            x = cs.vertcat(x, x_k)
+            xa = cs.vertcat(xa, xa_k)
+            s = cs.vertcat(s, s_k)
+            ea = cs.vertcat(ea, ea_k)
+            obs_val = cs.vertcat(obs_val, obs_val_k)
+            obs_val_a = cs.vertcat(obs_val_a, obs_val_a_k)
+            dlp = cs.vertcat(dlp, dlp_k)
+
+        # Define constraint vector
+        con = cs.vertcat(s, x)
+        if self.build_params['xaeN_con']:
+            con = cs.vertcat(con, x_k-xa_k)
+        if self.build_params['dlp_con']:
+            con = cs.vertcat(con, dlp)
+        # con = cs.vertcat(s, x, x_k-xa_k, dlp)
+        # con = cs.vertcat(s, x, x_k-xa_k)
+
+        u_target = cs.SX([self.build_params['w_max'], 0])
+        u_err = cs.repmat(u_target,self.build_params['N']) - ua
+        xae = x - xa
+        uae = u - ua
+        we = w - self.build_params['w_max']
+        # Define costs
+        Q = cs.repmat(cost_params['Q'], self.build_params['N'] + 1)
+        R = cs.repmat(cost_params['R'], self.build_params['N'])
+        K = cs.repmat(cost_params['K'], self.build_params['N'] + 1)
+        S = cs.repmat(cost_params['S'], self.build_params['N'])
+        T = cs.repmat(cost_params['T'], self.build_params['N'])
+        # Note s_cost is normalized for easier tuning
+        la_cost = cs.dot(Q, xae * xae) + cs.dot(R, uae * uae)
+        lo_cost = cs.dot(K, ea * ea) + cs.dot(S, u_err * u_err)
+        lw_cost = cs.dot(T, we ** 2)
+        obs_cost = 0.5 * cost_params['mu'] * (cs.sum1(obs_val * obs_val) + cs.sum1(obs_val_a * obs_val_a))
+        # Define constraints
+        self.cs_sol2state = cs.Function('cs_sol2state', [sol_sym, x0_sym, path_pol_sym], [x, s, xa], ['sol', 'x0', 'path_pol'], ['x', 's', 'xa'])
+        self.cs_sol2cost = cs.Function('cs_sol2cost', [sol_sym, x0_sym, path_pol_sym, obs_par_sym, cost_params_sym], [la_cost, lo_cost, lw_cost, obs_cost], ['sol', 'x0', 'path_pol', 'obs_par', 'cost_params'], ['la_cost', 'lo_cost', 'lw_cost', 'obs_cost'])
+        self.cs_sol2con = cs.Function('cs_sol2con', [sol_sym, x0_sym, path_pol_sym, cost_params_sym], [con], ['sol', 'x0', 'path_pol', 'cost_params'], ['con'])
+        self.cs_sol2obsval = cs.Function('cs_sol2obsval', [sol_sym, x0_sym, obs_par_sym], [obs_val], ['sol', 'x0', 'obs_par'], ['obs_val'])
+
+    def sol_bounds(self):
+        xa0_min = np.tile(-np.inf, self.build_params['nx'])
+        u_min = np.tile(self.build_params['u_min'], self.build_params['N'])
+        ua_min = np.tile(-np.inf, self.build_params['nu'] * self.build_params['N'])
+        w_min = np.zeros(self.build_params['N'])
+        xa0_max = np.tile(np.inf, self.build_params['nx'])
+        u_max = np.tile(self.build_params['u_max'], self.build_params['N'])
+        ua_max = np.tile(np.inf, self.build_params['nu'] * self.build_params['N'])
+        w_max = np.tile(self.build_params['w_max'], self.build_params['N'])
+        return np.concatenate((xa0_min, u_min, ua_min, w_min)), np.concatenate((xa0_max, u_max, ua_max, w_max))
+
+    def con_bounds(self):
+        s_min = np.zeros(self.build_params['N'] + 1)
+        x_min = np.tile(self.build_params['x_min'], self.build_params['N'] + 1)
+        xaeN_min = np.tile(-0.1, self.build_params['nx'])
+        dlp_min = np.tile(-np.inf, self.build_params['N'])
+        s_max = np.tile(self.build_params['w_max'] * self.build_params['dt'] * self.build_params['N'], self.build_params['N'] + 1)
+        x_max = np.tile(self.build_params['x_max'], self.build_params['N'] + 1)
+        xaeN_max = np.tile(0.1, self.build_params['nx'])
+        dlp_max = np.tile(0, self.build_params['N'])
+        con_min, con_max = np.concatenate((s_min, x_min)), np.concatenate((s_max, x_max))
+        if self.build_params['xaeN_con']:
+            con_min, con_max = np.concatenate((con_min, xaeN_min)), np.concatenate((con_max, xaeN_max))
+        if self.build_params['dlp_con']:
+            con_min, con_max = np.concatenate((con_min, dlp_min)), np.concatenate((con_max, dlp_max))
+        return con_min, con_max
+
+    def sol2state(self, sol, x0, path_pol):
+        x, s, xa = self.cs_sol2state(sol, x0, path_pol)
+        return np.array(x).flatten(), np.array(s).flatten(), np.array(xa).flatten()
+
+    def sol2con(self, sol, x0, path_pol, params):
+        cost_params = params['Q'] + params['R'] + [params['K']] + params['S'] + [params['T'], params['mu']]
+        return np.array(self.cs_sol2con(sol, x0, path_pol, cost_params)).flatten()
+
+    def sol2obsval(self, sol, x0, obs_par):
+        return np.array(self.cs_sol2obsval(sol, x0, obs_par)).flatten()
+
+    def sol2cost(self, sol, x0, path_pol, obs_par, params):
+        cost_params = params['Q'] + params['R'] + [params['K']] + params['S'] + [params['T'], params['mu']]
+        la_cost, lo_cost, lw_cost, obs_cost = self.cs_sol2cost(sol, x0, path_pol, obs_par, cost_params)
+        return {'la_cost': float(la_cost), 'lo_cost': float(lo_cost), 'lw_cost': float(lw_cost), 'obs_cost': float(obs_cost)}
+
+    def sol2xa0uuaw(self, sol):
+        xa0_len, u_len, ua_len, w_len = self.build_params['nx'], self.build_params['nu'] * self.build_params['N'], \
+                                        self.build_params['nu'] * self.build_params['N'], self.build_params['N']
+        u_idx = xa0_len
+        ua_idx = u_idx + u_len
+        w_idx = ua_idx + ua_len
+        xa0 = sol[:u_idx]
+        u = sol[u_idx:ua_idx]
+        ua = sol[ua_idx:w_idx]
+        w = sol[w_idx:]
+        return xa0, u, ua, w
+
+    def build(self):
+        # Build parametric optimizer
+        # ------------------------------------
+
+        n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict'])
+        n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2
+
+        params = {'x0': self.build_params['nx'], 'path_pol': self.build_params['np'] * (self.build_params['n_pol'] + 1),
+                  'Q': self.build_params['nx'], 'R': self.build_params['nu'], 'K': 1,
+                   'S': self.build_params['nu'], 'T': 1, 'mu': 1,
+                  'obs_par': n_ell_par + n_pol_par}
+        par_dim = sum(list(params.values()))
+        # Exchange parameter dimension with value
+        par = cs.SX.sym("par", par_dim)  # Parameters
+        p_idx = 0
+        for key, dim in params.items():
+            params[key] = par[p_idx:p_idx + dim]
+            p_idx += dim
+
+        # Define solution vector
+        sol = cs.SX.sym('sol', self.build_params['nx'] + (2 * self.build_params['nu'] + 1) * self.build_params['N'])
+
+        # Cost
+        cost_params = cs.vertcat(params['Q'], params['R'], params['K'], params['S'], params['T'], params['mu'])
+        la_cost, lo_cost, lw_cost, obs_cost = self.cs_sol2cost(sol, params['x0'], params['path_pol'], params['obs_par'], cost_params)
+        cost = la_cost + lo_cost + lw_cost + obs_cost
+
+        # Constraints
+        sol_min, sol_max = self.sol_bounds()
+        con_min, con_max = self.con_bounds()
+        sol_bounds = og.constraints.Rectangle(sol_min.tolist(), sol_max.tolist())
+        agl_con = self.cs_sol2con(sol, params['x0'], params['path_pol'], cost_params)
+        agl_con_bounds = og.constraints.Rectangle(con_min.tolist(), con_max.tolist())
+        # Setup builder
+        problem = og.builder.Problem(sol, par, cost) \
+            .with_constraints(sol_bounds) \
+            .with_aug_lagrangian_constraints(agl_con, agl_con_bounds)
+        build_config = og.config.BuildConfiguration() \
+            .with_build_directory(self.build_dir) \
+            .with_build_mode(self.build_params['mode']) \
+            .with_build_python_bindings()
+        meta = og.config.OptimizerMeta() \
+            .with_optimizer_name(self.build_name)
+        solver_config = og.config.SolverConfiguration() \
+            .with_tolerance(self.build_params['solver_tol']) \
+            .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \
+            .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \
+            .with_max_outer_iterations(self.build_params['solver_max_outer_iterations'])
+        builder = og.builder.OpEnOptimizerBuilder(problem,
+                                                  meta,
+                                                  build_config,
+                                                  solver_config) \
+            .with_verbosity_level(1)
+        builder.build()
+        print('')
+
+        # Save build params
+        with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file:
+            yaml.dump(self.build_params, file)
+
+
+    def run(self, x0, path_pol, params, obs_par, init_guess):
+        p = x0 + path_pol + params['Q'] + params['R'] + [params['K']] + params['S'] + [params['T'], params['mu']] + obs_par
+        if init_guess is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, 2*e_max, lam_rho):
+            # Use base solution as initial guess
+            init_guess = self.base_solution(x0, path_pol, 0.1)
+
+        # Run solver
+        solution_data = self.solver.run(p=p, initial_guess=init_guess)
+
+        return solution_data
+
+        base_sol = [0] * (self.build_params['nx'] + (2 * self.build_params['nu'] + 1) * self.build_params['N'])
+        base_sol[:self.build_params['nx']] = x0
+        base_sol[-self.build_params['N']] = self.build_params['dp_max']
+
+
+        if self.sol_prev is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, params):
+            # Use base solution as initial guess
+            self.sol_prev = base_sol
+
+        # Run solver
+        solution_data = self.solver.run(p=p, initial_guess=self.sol_prev)
+
+        if solution_data is None:
+            sol, exit_status = base_sol, "TrivialSolution"
+            self.sol_prev = None
+        else:
+            sol, exit_status = solution_data.solution, solution_data.exit_status
+            # self.sol_prev = sol
+            x, s, xa = self.sol2state(sol, x0, path_pol)
+
+            xa0, u, ua, w = self.sol2xa0uuaw(sol)
+
+            self.sol_prev = xa[self.build_params['nx']:2*self.build_params['nx']].tolist() + \
+                            u[self.build_params['nu']:] + [0] * self.build_params['nu'] + \
+                            ua[self.build_params['nu']:] + [0] * self.build_params['nu'] + \
+                            w[1:] + [0]
+
+        sol_feasible = self.is_feasible(sol, x0, path_pol, params, d=verbosity>0)
+
+        return sol, sol_feasible, exit_status
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_artificial_reference/mpc_build/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py
new file mode 100644
index 0000000..04c4fb7
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/__init__.py
@@ -0,0 +1,5 @@
+from .workspace_modification import workspace_modification, rho_environment, extract_r0
+from .path_generator import path_generator
+from .mpc import Mpc, pol2pos
+# from .tunnel_mpc_controller import TunnelMpcController
+from .motion_controller import MotionController, ControlMode
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py
new file mode 100644
index 0000000..36f7fd8
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/motion_controller.py
@@ -0,0 +1,447 @@
+import matplotlib.pyplot as plt
+
+from motion_control.pfmpc_ds import workspace_modification, path_generator, Mpc, pol2pos, rho_environment, extract_r0
+from starworlds.utils.misc import tic, toc
+from starworlds.obstacles import Ellipse, StarshapedPolygon, motion_model
+import numpy as np
+import shapely
+import warnings
+from enum import Enum
+
+class ControlMode(Enum):
+    SBC = 1
+    MPC = 2
+
+    class InvalidModeError(Exception):
+        pass
+
+
+class MotionController:
+    # reference_path: list of waypoints (i.e. list of lists)
+    def __init__(self, params, robot, reference_path, verbosity=0):
+        self.params = params
+        self.robot = robot
+        self.mpc = Mpc(params, robot)
+        self.rhrp_L = self.params['N'] * self.params['dt'] * self.mpc.build_params['w_max']
+        self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps'])
+        self.set_reference_path(reference_path)
+        self.verbosity = verbosity
+        # print(self.params['cs'] * self.mpc.build_params['w_max'], self.params['ce'] * self.params['rho0'])
+        self.reset()
+
+    def reset(self):
+        self.mpc.reset()
+        self.mode = ControlMode.SBC
+        self.obstacle_clusters = None
+        self.obstacles_rho = []
+        self.workspace_rho = None
+        self.obstacles_star = []
+        self.r_plus = None
+        self.theta = 0
+        self.ds_generated_path = None
+        self.rg = None
+        self.pg_buffer = None
+        self.pg = None
+        self.rhrp_path = None
+        self.path_pol = None
+        self.rho = None
+        self.epsilon = None
+        self.solution = None
+        self.sol_feasible = None
+        self.mpc_exit_status = None
+        if self.params['lambda'] * self.params['rho0'] > self.mpc.build_params['w_max'] * self.params['dt']:
+            print("Poor lamda, rho0 selection. lambda*rho ({:.2f}) > w_max*dt ({:.2f})".format(self.params['lambda'] * self.params['rho0'], self.mpc.build_params['w_max'] * self.params['dt']))
+        # else:
+        #     print("Ok lamda, rho0 selection. {:.2f} <= {:.2f}".format(self.params['lambda'] * self.params['rho0'],
+        #                                                               self.mpc.build_params['dp_max']))
+        self.u_prev = [0] * self.robot.nu
+        self.timing = {'workspace': 0, 'target': 0, 'mpc': 0, 'workspace_detailed': [0] * 4}
+
+    def set_reference_path(self, reference_path):
+        self.reference_path = shapely.geometry.Point(reference_path[0]) if len(reference_path) == 1 else shapely.geometry.LineString(reference_path)
+        self.theta_g = self.reference_path.length
+
+    # Extract path rs -> reference_path(theta, theta+L)
+    def nominal_rhrp(self, r0, free_rho_sh):
+        # Find receding global reference
+        # nominal_rhrp_0 = np.array(self.reference_path.interpolate(self.theta).coords[0])
+        nominal_rhrp_sh = shapely.ops.substring(self.reference_path, start_dist=self.theta, end_dist=self.theta + self.params['nominal_rhrp_horizon'])
+        init_path = shapely.geometry.LineString([r0, nominal_rhrp_sh.coords[0]])
+        if nominal_rhrp_sh.geom_type == 'Point':
+            nominal_rhrp_sh = init_path
+        else:
+            nominal_rhrp_sh = shapely.ops.linemerge([init_path, nominal_rhrp_sh])
+        nominal_rhrp_sh = shapely.ops.substring(nominal_rhrp_sh, start_dist=0, end_dist=self.params['nominal_rhrp_horizon'])
+        path_collision_check = nominal_rhrp_sh if nominal_rhrp_sh.length > init_path.length else init_path
+        collision_free = path_collision_check.within(free_rho_sh)
+        return nominal_rhrp_sh, collision_free
+
+    def ref_kappa(self, p, r0, r_lambda_rho, rho):
+        if np.linalg.norm(p-r_lambda_rho) < rho:
+            return r_lambda_rho
+        # Intersection point of B(r0,rho) and B(rlr,rho)
+        rc = 0.5 * (r0 + r_lambda_rho)
+        nc = np.array([rc[1] - r0[1], r0[0] - rc[0]])
+        nc /= np.linalg.norm(nc)
+        d = np.sqrt(rho ** 2 - np.linalg.norm(rc - r0) ** 2)
+        i1 = rc + nc * d
+        i2 = rc - nc * d
+        # Line coefficients for l[r0, r_lambda_rho]
+        a = r0[1] - r_lambda_rho[1]
+        b = r_lambda_rho[0] - r0[0]
+        c = - (r0[1] * b + r0[0] * a)
+        # Line coefficients for li
+        i_scale = 0.01
+        ai = i1[0] - p[0]
+        bi = i1[1] - p[1]
+        ci = 0.5 * (p.dot(p) - i1.dot(i1) + i_scale * (p.dot(i2) - p.dot(i1) - i1.dot(i2) + i1.dot(i1)))
+        # Return intersection of lines
+        return np.array([(b * ci - bi * c) / (a * bi - ai * b), (c * ai - ci * a) / (a * bi - ai * b)])
+
+    def unicycle_sbc(self, x, p_ref):
+        k1, k2 = 2, 2
+        # k1, k2 = 1.25, 0.3
+        e = np.array(x[:2]) - np.array(p_ref)
+        th = x[2]
+
+        r_th = np.arctan2(e[1], e[0]) + np.pi
+        # E Theta in (-pi, pi]
+        e_th = r_th - th
+        e_th0 = e_th
+        while e_th > np.pi:
+            e_th -= 2 * np.pi
+        while e_th <= -np.pi:
+            e_th += 2 * np.pi
+
+        u = [
+            -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)),
+            k2 * (e_th)
+        ]
+
+        e_norm = np.linalg.norm(e)
+        if e_norm < abs(u[0]) * self.params['dt']:
+            u[0] = np.sign(u[0]) * e_norm / self.params['dt']
+
+        if self.robot.u_min[0] == 0 and u[0] < 0:
+            u[0] = 0
+            u0_sig = 1
+        else:
+            u0_sig = u[0] / self.robot.u_max[0] if u[0] >= 0 else u[0] / self.robot.u_min[0]
+        u1_sig = u[1] / self.robot.u_max[1] if u[1] >= 0 else u[1] / self.robot.u_min[1]
+
+        sig = max(u0_sig, u1_sig, 1)
+        if sig > 1:
+            if u0_sig > u1_sig:
+                u[0] = self.robot.u_max[0] if u[0] > 0 else self.robot.u_min[0]
+                u[1] /= sig
+            else:
+                u[0] /= sig
+                u[1] =  self.robot.u_max[1] if u[1] > 0 else self.robot.u_min[1]
+
+        return u
+
+        # Control law (Assumes Unicycle robot)
+        ref_dist = np.linalg.norm(x[:2] - p_ref)
+        if ref_dist > 1e-5:
+            theta_ref = np.arctan2(p_ref[1] - x[1], p_ref[0] - x[0])
+            theta_diff = float(theta_ref - x[-1])
+            if theta_diff > np.pi:
+                theta_diff -= 2 * np.pi
+            if theta_diff < -np.pi:
+                theta_diff += 2 * np.pi
+            if abs(theta_diff) < 1e-2:  # Less than 0.57 degree error
+                # Linear velocity
+                u[0] = min(self.robot.u_max[0], ref_dist / self.params['dt'])
+            else:
+                # Angular velocity
+                if theta_diff > 0:
+                    u[1] = min(self.robot.u_max[1], theta_diff / self.params['dt'])
+                else:
+                    u[1] = max(self.robot.u_min[1], theta_diff / self.params['dt'])
+        return u
+
+    def r_plus_dist(self):
+        return np.linalg.norm(np.array(self.reference_path.interpolate(self.theta).coords[0]) - self.r_plus)
+
+    def theta_plus(self, s):
+        return max(self.theta, min(self.theta + s - self.r_plus_dist(), self.theta_g))
+
+    def compute_u(self, x):
+        if self.mode == ControlMode.MPC:
+            u, w = self.mpc.sol2uds(self.solution)
+            return u[:self.robot.nu]
+        else:
+            # r_kappa = self.ref_kappa(self.robot.h(x), self.rhrp_path[0, :],
+            #                          pol2pos(self.path_pol, self.lam_rho, self.mpc.build_params['n_pol']), self.rho)
+            r_kappa = self.rhrp_path[0, :]
+            if self.robot.__class__.__name__ == 'Unicycle':
+                return self.unicycle_sbc(x, r_kappa)
+            elif self.robot.__class__.__name__ == 'Omnidirectional':
+                k = 100
+                u = [k*(r_kappa[0]-x[0]), k*(r_kappa[1]-x[1])]
+                for i in range(2):
+                    scale = 1
+                    if u[i] > self.robot.u_max[i]:
+                        scale = self.robot.u_max[i] / u[i]
+                    elif u[i] < self.robot.u_min[i]:
+                        scale = self.robot.u_min[i] / u[i]
+                    u = [scale * u[0], scale * u[1]]
+                #
+                # u0_sig = u[0] / self.robot.u_max[0]
+                # u1_sig = u[1] / self.robot.u_max[1]
+                # sig = max(u0_sig, u1_sig, 1)
+                # if sig > 1:
+                #     if u0_sig > u1_sig:
+                #         u[0] = self.robot.u_max[0]
+                #         u[1] /= sig
+                #     else:
+                #         u[0] /= sig
+                #         u[1] = self.robot.u_max[1]
+                return u
+            else:
+                print("No SBC for this robot model!!!!")
+
+    def update_policy(self, x, obstacles, workspace=None):
+        if workspace is None:
+            workspace = Ellipse([1.e10, 1.e10])
+        p = self.robot.h(x)
+
+        obstacles_local = obstacles.copy()
+        # Adjust for robot radius
+        if self.robot.radius > 0:
+            obstacles_local, workspace, _, _ = rho_environment(workspace, obstacles_local, self.robot.radius)
+
+        # Adjust for moving obstacles
+        if self.params['velocity_obstacle']:
+            for i, o in enumerate(obstacles_local):
+                if not (o._motion_model is None or o._motion_model.__class__.__name__ == "Static"):
+                    o_pol = o.polygon()
+                    xvel, yvel = o._motion_model.lin_vel()
+                    dil = StarshapedPolygon(shapely.MultiPolygon([o_pol, shapely.affinity.translate(o_pol, xoff=xvel*self.params['dt'], yoff=yvel*self.params['dt'])]).convex_hull)
+                    # dil = o.dilated_obstacle(padding=max(abs(o._motion_model.lin_vel())) * self.params['dt'], id="duplicate")
+                    if dil.polygon().distance(shapely.geometry.Point(p)) > max(abs(o._motion_model.lin_vel()))*self.params['dt']:
+                        obstacles_local[i] = dil
+                    else:
+                        print("Not dillated obstacle.")
+                    # dil = o.dilated_obstacle(padding=max(abs(o._motion_model.lin_vel())) * self.params['dt'], id="duplicate")
+                    # _, ax = o.draw()
+                    # ax.set_xlim([-1, 12])
+                    # ax.set_ylim([-8, 4])
+                    # obstacles_local[i].draw(ax=ax, fc='r', alpha=0.4, zorder=-2)
+                    # dil.draw(ax=ax, fc='b', alpha=0.4, zorder=-3)
+                    # plt.show()
+
+        if self.params['workspace_horizon'] > 0:
+            obstacle_detection_region = shapely.geometry.Point(p).buffer(self.params['workspace_horizon'])
+            obstacles_filtered = []
+            for o in obstacles_local:
+                if obstacle_detection_region.intersects(o.polygon()):
+                    obstacles_filtered += [o]
+            obstacles_local = obstacles_filtered
+        else:
+            obstacles_filtered = []
+            for o in obstacles_local:
+                if workspace.polygon().intersects(o.polygon()):
+                    obstacles_filtered += [o]
+            obstacles_local = obstacles_filtered
+
+        # Initialize rs to robot position
+        if self.r_plus is None:
+            self.r_plus = p
+
+        self.timing['workspace'] = 0
+        self.timing['target'] = 0
+        self.timing['mpc'] = 0
+        self.timing['workspace_detailed'] = None
+
+        pg_prev = self.pg
+        self.rg = None
+        self.pg = None
+        self.rho = self.params['rho0']
+        ref_end = np.array(self.reference_path.coords[-1])
+        close_to_convergence = (np.linalg.norm(self.r_plus - ref_end) < self.rho) and (self.theta >= self.theta_g)
+        if close_to_convergence:
+            self.mode = ControlMode.SBC
+            for i in range(len(self.rhrp_s)):
+                self.rhrp_path[i, :] = ref_end
+        else:
+            ds_path_generation = True
+            if self.theta_g > 0:
+                # Extract receding path from global target path
+                t0 = tic()
+                obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh = rho_environment(workspace, obstacles_local, self.rho)
+                self.timing['workspace'] = toc(t0)
+
+                t0 = tic()
+                nominal_rhrp_sh, nominal_rhrp_collision_free = self.nominal_rhrp(self.r_plus, free_rho_sh)
+                if nominal_rhrp_collision_free:
+                    ds_path_generation = False
+                    if nominal_rhrp_sh.length > 0:
+                        self.rhrp_path = np.array([nominal_rhrp_sh.interpolate(s).coords[0] for s in self.rhrp_s])
+                    else:
+                        self.rhrp_path = np.tile(nominal_rhrp_sh.coords[0], (len(self.rhrp_s), 1))
+                    self.obstacles_rho = obstacles_rho
+                    self.workspace_rho = workspace_rho
+                    self.obstacles_star = []
+                    self.obstacle_clusters = None
+                    self.pg = self.rhrp_path[-1, :]
+                    rhrp_path_length = self.rhrp_L
+                self.timing['target'] = toc(t0)
+
+            if ds_path_generation:
+                # Find attractor for DS dynamics
+                if self.theta_g == 0:
+                    self.pg = np.array(self.reference_path.coords[0])
+                else:
+                    t0 = tic()
+                    self.theta = self.theta_plus(self.params['nominal_rhrp_horizon'])
+                    for theta in np.arange(self.theta_plus(self.params['nominal_rhrp_horizon']), self.theta_g, np.diff(self.rhrp_s[:2])):
+                        # if self.reference_path.interpolate(theta).disjoint(obstacles_rho_sh):
+                        if self.reference_path.interpolate(theta).within(free_rho_sh):
+                            self.theta = theta
+                            break
+                    self.pg = np.array(self.reference_path.interpolate(self.theta).coords[0])
+                    print(self.theta_g, theta, self.reference_path.interpolate(theta).within(free_rho_sh))
+                    self.timing['workspace'] += toc(t0)
+
+                pg_buffer_thresh = 0.5
+                # new_pg_buffer = not self.ds_generated_path or np.linalg.norm(self.pg_buffer - self.pg) > pg_buffer_thresh
+                # if new_pg_buffer:
+                #     self.pg_buffer = self.pg
+
+                buffer_active = self.params['buffer'] and (self.ds_generated_path and np.linalg.norm(pg_prev - self.pg) < pg_buffer_thresh)
+                buffer_path = self.rhrp_path if buffer_active else None
+                if buffer_active:
+                    self.pg_buffer = self.pg
+
+                local_pars = self.params.copy()
+                local_pars['max_rhrp_compute_time'] -= self.timing['target']
+
+                self.rhrp_path, rhrp_path_length, self.obstacle_clusters, self.obstacles_rho, self.workspace_rho, \
+                self.obstacles_star, self.rho, self.rg, workspace_exitflag, workspace_timing, target_timing, self.timing['workspace_detailed'] = \
+                    ds_path_gen(obstacles_local, workspace, p, self.pg, self.r_plus, self.params['rho0'], self.params['hull_epsilon'], self.rhrp_s,
+                                self.obstacle_clusters, buffer_path, local_pars, self.verbosity)
+
+                # Update O+
+                t0 = tic()
+                obstacles_rho_sh = shapely.ops.unary_union([o.polygon() for o in self.obstacles_rho])
+                C_rho0 = self.workspace_rho.polygon().difference(obstacles_rho_sh).buffer(self.params['rho0'])
+                env_DSW = workspace_exitflag != 0
+                if not (env_DSW and (shapely.geometry.Point(p).within(C_rho0) or self.rho == self.params['rho0'])):
+                    self.obstacle_clusters = None
+                self.timing['workspace'] += toc(t0)
+
+                self.timing['workspace'] += workspace_timing
+                self.timing['target'] += target_timing
+
+            self.ds_generated_path = ds_path_generation
+
+            t0 = tic()
+            # Approximate target with polynomials
+            self.path_pol = np.polyfit(self.rhrp_s, self.rhrp_path[:, 0], self.params['n_pol']).tolist() + \
+                            np.polyfit(self.rhrp_s, self.rhrp_path[:, 1], self.params['n_pol']).tolist()
+            # Force init position to be correct
+            self.path_pol[self.params['n_pol']] = self.rhrp_path[0, 0]
+            self.path_pol[-1] = self.rhrp_path[0, 1]
+            # Compute polyfit approximation error
+            self.epsilon = max(np.linalg.norm(self.rhrp_path - np.array([pol2pos(self.path_pol, s, self.mpc.build_params['n_pol']) for s in self.rhrp_s]), axis=1))
+            self.timing['target'] += toc(t0)
+
+            t0 = tic()
+            self.lam_rho = self.params['lambda'] * self.rho  # Parameter for tracking error constraint
+
+            # Compute MPC solution
+            e_max = self.rho - self.epsilon
+            solution_data = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, e_max, self.lam_rho, self.solution, verbosity=self.verbosity)
+            if solution_data is None:
+                self.solution, self.mpc_exit_status, self.sol_feasible = None, "BaseSolution", False
+            else:
+                self.solution, self.mpc_exit_status = solution_data.solution, solution_data.exit_status
+                self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, e_max, self.lam_rho, d=self.verbosity > 0)
+            # self.solution, self.sol_feasible, self.mpc_exit_status = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params,
+            #                                                e_max, self.lam_rho, verbosity=self.verbosity)
+            self.timing['mpc'] = toc(t0)
+
+            if (np.linalg.norm(self.rhrp_path[-1, :]-self.pg) <= 0.1 * self.rhrp_L or rhrp_path_length > 0.1 * self.rhrp_L) and self.sol_feasible:
+                self.mode = ControlMode.MPC
+            else:
+                if self.verbosity > 0:
+                    print("[Motion Controller]: MPC solution not feasible. Using default control law.")
+                self.mode = ControlMode.SBC
+
+        if self.mode == ControlMode.SBC:
+            # r_kappa = self.ref_kappa(p, self.rhrp_path[0, :], pol2pos(self.path_pol, self.lam_rho, self.mpc.build_params['n_pol']), self.rho)
+            # r_kappa = self.rhrp_path[0, :]
+            # u = self.unicycle_sbc(x, r_kappa)
+            # if self.verbosity > 0:
+            #     print("[Motion Controller]: MPC solution not feasible. Using default control law. u = " + str(u))
+            self.r_plus = self.rhrp_path[0, :]
+        else:
+            u, w = self.mpc.sol2uds(self.solution)
+            # u = u[:self.robot.nu]
+            # Update rs and theta
+
+            # if np.linalg.norm(self.rhrp_path[0, :] - np.array(self.reference_path.interpolate(self.theta).coords[0])) < self.rho:
+            if self.theta_g > 0 and self.r_plus_dist() < self.rho:
+                self.theta = min(self.theta + w[0] * self.params['dt'], self.theta_g)
+
+            self.r_plus = np.array(pol2pos(self.path_pol, w[0] * self.params['dt'], self.mpc.build_params['n_pol']))
+            # if update_theta:
+            # if self.theta < self.theta_g and np.linalg.norm(self.rhrp_path[0, :] - np.array(self.reference_path.interpolate(self.theta).coords[0])) < self.rho:
+            #     self.theta += w[0] * self.params['dt']
+        # print(np.linalg.norm(self.r_plus-self.rhrp_path[0, :]) )
+
+
+        # return self.sppc(x, pol2pos(self.path_pol, self.lam_rho, self.mpc.build_params['n_pol']))
+        # return self.sppc(x, self.reference_path.coords[-1])
+
+        # self.sol_feasible = False
+
+        self.u_prev = self.compute_u(x)
+        # return np.array(u)
+
+
+def ds_path_gen(obstacles, workspace, p, pg, r_plus, rho0, hull_epsilon, rhrp_s,
+                previous_obstacle_clusters=None, buffer_path=None, params=None, verbosity=0):
+    # Update obstacles
+    obstacle_clusters, r0, rg, rho, obstacles_rho, workspace_rho, workspace_timing, workspace_timing_detailed, workspace_exitflag = \
+        workspace_modification(obstacles, workspace, p, pg, r_plus, rho0, hull_epsilon, previous_obstacle_clusters, params, verbosity)
+    obstacles_star = [o.cluster_obstacle for o in obstacle_clusters]
+    # if exit_flag == 0:
+    #     obstacle_clusters = None
+
+    # Make sure all polygon representations are computed
+    [o._compute_polygon_representation() for o in obstacles_star]
+
+    # Buffer target path
+    init_path, init_s = None, None
+    if buffer_path is not None:
+        # Shift path to start closest to current r_plus
+        init_path = buffer_path[np.argmin(np.linalg.norm(r0 - buffer_path, axis=1)):, :]
+        if np.linalg.norm(r0 - init_path[0, :]) > 1e-6:
+            init_path = np.vstack((r0, init_path))
+
+        # ||self.r_plus - p|| < rho from construction
+        for r in init_path:
+            # NOTE: Assumes previous path not outside workspace due to static workspace
+            if not all([o.exterior_point(r) for o in obstacles_star]) or not workspace_rho.interior_point(r):
+            # if not all([o.exterior_point(r) for o in obstacles_star]):
+                if verbosity > 1:
+                    print("[Path Generator]: No reuse of previous path. Path not collision-free.")
+                init_path = None
+                break
+
+        if init_path is not None:
+            # Cut off stand still padding in init path
+            init_path_stepsize = np.linalg.norm(np.diff(init_path, axis=0), axis=1)
+            init_s = np.hstack((0, np.cumsum(init_path_stepsize)))
+            init_path_mask = [True] + (init_path_stepsize > 1e-8).tolist()
+            init_path = init_path[init_path_mask, :]
+            init_s = init_s[init_path_mask]
+
+
+    # Update target path
+    rhrp_path, rhrp_path_length, path_timing = \
+        path_generator(r0, rg, obstacles_star, workspace_rho, rhrp_s, init_path, init_s, params, verbosity)
+
+    return rhrp_path, rhrp_path_length, obstacle_clusters, obstacles_rho, workspace_rho, obstacles_star, rho, rg, workspace_exitflag, workspace_timing, path_timing, workspace_timing_detailed
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py
new file mode 100644
index 0000000..3503887
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc.py
@@ -0,0 +1,364 @@
+import casadi.casadi as cs
+import opengen as og
+import numpy as np
+import os, sys
+import yaml
+
+
+class NoSolutionError(Exception):
+    '''raise this when there's no solution to the mpc problem'''
+    pass
+
+def pol_eval(pol_coeff, x, n_pol):
+    return sum([pol_coeff[i] * x ** (n_pol - i) for i in range(n_pol+1)])
+
+
+def pol2pos(pol_coeff, s, n_pol):
+    return [pol_eval(pol_coeff[:n_pol+1], s, n_pol), pol_eval(pol_coeff[n_pol+1:], s, n_pol)]
+
+
+class Mpc:
+
+    def __init__(self, params, robot):
+        self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build')
+        # Load parameters
+        self.build_params = None
+        self.robot = robot
+        self.set_build_params(params)
+        rebuild, self.build_name = self.get_build_version()
+        self.generate_sol_fun()
+        # Build if different from latest build
+        if rebuild:
+            self.build()
+        else:
+            print("Found MPC build: {}".format(self.build_name))
+
+        sys.path.insert(1, os.path.join(self.build_dir, self.build_name))
+        optimizer = __import__(self.build_name)
+        self.solver = optimizer.solver()
+        self.reset()
+
+    def reset(self):
+        self.sol_prev = None
+
+    def set_build_params(self, params):
+        self.build_params = {
+            'mode': params['build_mode'],
+            'N': params['N'],
+            'dt': params['dt'],
+            'solver_tol': params['solver_tol'],
+            'solver_max_time': params['solver_max_time'],
+            'solver_max_inner_iterations': params['solver_max_inner_iterations'],
+            'solver_max_outer_iterations': params['solver_max_outer_iterations'],
+            'x_min': self.robot.x_min,
+            'x_max': self.robot.x_max,
+            'u_min': self.robot.u_min,
+            'u_max': self.robot.u_max,
+            'nx': self.robot.nx,
+            'nu': self.robot.nu,
+            'np': 2,
+            'robot_model': self.robot.__class__.__name__,
+            'n_pol': params['n_pol'],
+            'integration_method': params['integration_method'],
+            'w_max': self.robot.vmax
+        }
+
+    def get_build_version(self):
+        builds = [name for name in os.listdir(self.build_dir)
+                  if os.path.isdir(os.path.join(self.build_dir, name))]
+        for build_ver in builds:
+            if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')):
+                continue
+            with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file:
+                build_ver_params = yaml.load(file, Loader=yaml.FullLoader)
+            if self.build_params == build_ver_params:
+                return False, build_ver
+        ver = 0
+        while 'ver' + str(ver) in os.listdir(self.build_dir):
+            ver += 1
+        return True, 'ver' + str(ver)
+
+    def is_feasible(self, sol, x0, path_pol, e_max, lam_rho, d=False):
+        u, w = self.sol2uds(sol)
+        x, s, e = self.sol2state(sol, x0, path_pol)
+        u_min, w_min, s1_lamrho_min, sN_min, x_min, e_max_min = self.min_bounds()
+        u_max, w_max, s1_lamrho_max, sN_max,  x_max, e_max_max = self.max_bounds()
+
+        eps = 1e-3
+
+        sol_ok = True
+        if not all((u_min <= u) & (u <= u_max)):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad u")
+                print(u)
+        if not all((w_min <= w) & (w <= w_max)):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad w")
+                print(w)
+        if not all((x_min <= x) & (x <= x_max)):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad x")
+                print(x)
+        if s1_lamrho_min - eps > s[1] - lam_rho:
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad s1 {:.4f} < {:.4f}".format(s[1], lam_rho))
+        if not ((sN_min <= s[-1]) and (s[-1] <= sN_max + 0.1)):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad sN {:.4f} > {:.4f}".format(s[-1], sN_max))
+        if not all((e_max_min <= e - e_max) & (e - e_max <= e_max_max)):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad e (e_max: {:.4f})".format(e_max))
+                print(e)
+        return sol_ok
+
+    def error(self, x, s, path_pol):
+        p_ref = cs.vertcat(*pol2pos(path_pol, s, self.build_params['n_pol']))
+        return cs.norm_2(p_ref - self.robot.h(x))
+
+    def base_solution(self, x0, path_pol, lam_rho):
+        u = [0] * (self.build_params['N'] * self.build_params['nu'])
+        w = [0] * (self.build_params['N'])
+        w[0] = lam_rho / self.build_params['dt']
+
+        if self.build_params['robot_model'] == 'Unicycle':
+            p_ref = pol2pos(path_pol, lam_rho, self.build_params['n_pol'])
+            k1, k2 = 2, 2
+            # k1, k2 = 1.25, 0.3
+
+            x = x0.copy()
+            for i in range(self.build_params['N']):
+                e = np.array(x[:2]) - np.array(p_ref)
+                th = x[2]
+
+                r_th = np.arctan2(e[1], e[0]) + np.pi
+                # E Theta in (-pi, pi]
+                e_th = r_th - th
+                e_th0 = e_th
+                while e_th > np.pi:
+                    e_th -= 2 * np.pi
+                while e_th <= -np.pi:
+                    e_th += 2 * np.pi
+
+                ui = [
+                    -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)),
+                    k2 * (e_th)
+                ]
+
+                if self.robot.u_min[0] == 0 and ui[0] < 0:
+                    ui[0] = 0
+                    u0_sig = 1
+                else:
+                    u0_sig = ui[0] / self.robot.u_max[0] if ui[0] >= 0 else ui[0] / self.robot.u_min[0]
+                u1_sig = ui[1] / self.robot.u_max[1] if ui[1] >= 0 else ui[1] / self.robot.u_min[1]
+
+                sig = max(u0_sig, u1_sig, 1)
+                if sig > 1:
+                    if u0_sig > u1_sig:
+                        ui[0] = self.robot.u_max[0] if ui[0] > 0 else self.robot.u_min[0]
+                        ui[1] /= sig
+                    else:
+                        ui[0] /= sig
+                        ui[1] = self.robot.u_max[1] if ui[1] > 0 else self.robot.u_min[1]
+                u[i * self.build_params['nu']:(i+1) * self.build_params['nu']] = ui
+
+                x, _ = self.robot.move(x, ui, self.build_params['dt'])
+
+        return u + w
+
+    def discrete_integration(self, f, x, u):
+        dt = self.build_params['dt']
+        if self.build_params['integration_method'] == 'euler':
+            return x + f(x, u) * dt
+        if self.build_params['integration_method'] == 'RK4':
+            k1 = f(x, u)
+            k2 = f(x + dt / 2 * k1, u)
+            k3 = f(x + dt / 2 * k2, u)
+            k4 = f(x + dt * k3, u)
+            return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
+        raise NotImplemented
+
+    def generate_sol_fun(self):
+        # Initialize
+        sol_sym = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N'])
+        x0_sym = cs.SX.sym('x0', self.build_params['nx'])
+        path_pol_sym = cs.SX.sym('path_pol', self.build_params['np'] * (self.build_params['n_pol'] + 1))
+        cost_params = {'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'],
+                       'u_prev': self.build_params['nu']}
+        cost_params_sym = cs.SX.sym("cost_params", sum(list(cost_params.values())))
+        # Exchange parameter dimension with SX variable
+        p_idx = 0
+        for key, dim in cost_params.items():
+            cost_params[key] = cost_params_sym[p_idx:p_idx + dim]
+            p_idx += dim
+        # Initialize
+        u, w = self.sol2uds(sol_sym)
+        x_k, s_k = x0_sym, 0
+        e_k = self.error(x_k, s_k, path_pol_sym)
+        x, s, e = x_k, s_k, e_k
+        # Loop over time steps
+        for k in range(self.build_params['N']):
+            # Current control variables
+            u_k = u[k * self.build_params['nu']:(k + 1) * self.build_params['nu']]
+            # Integrate one step
+            x_k = self.discrete_integration(lambda x, u: cs.vertcat(*self.robot.f(x,u)), x_k, u_k)
+            s_k = self.discrete_integration(lambda s, w: w, s_k, w[k])
+            e_k = self.error(x_k, s_k, path_pol_sym)
+            # Store current state
+            x = cs.vertcat(x, x_k)
+            s = cs.vertcat(s, s_k)
+            e = cs.vertcat(e, e_k)
+        # Define costs
+        u_target = cs.SX([self.build_params['w_max'], 0])
+        u_err = cs.repmat(u_target,self.build_params['N']) - u
+        du = u - cs.vertcat(cost_params['u_prev'], u[:-self.build_params['nu']])
+        R = cs.repmat(cost_params['R'], self.build_params['N'])
+        DR = cs.repmat(cost_params['DR'], self.build_params['N'])
+        w_cost = cost_params['cs'] * cs.sum1(self.build_params['w_max'] - w)
+        e_cost = cost_params['ce'] * cs.sum1(e)
+        # u_cost = cs.dot(R, u * u)
+        u_cost = cs.dot(R, u_err * u_err)
+        du_cost = cs.dot(DR, du * du)
+        # Define constraints
+        self.cs_sol2state = cs.Function('cs_sol2state', [sol_sym, x0_sym, path_pol_sym], [x, s, e], ['sol', 'x0', 'path_pol'], ['x', 's', 'e'])
+        self.cs_sol2cost = cs.Function('cs_sol2cost', [sol_sym, x0_sym, path_pol_sym, cost_params_sym], [w_cost, e_cost, u_cost, du_cost], ['sol', 'x0', 'path_pol', 'cost_params'], ['s_cost', 'e_cost', 'u_cost', 'du_cost'])
+
+    def min_bounds(self):
+        u_min = np.tile(self.build_params['u_min'], self.build_params['N'])
+        w_min = np.zeros(self.build_params['N'])
+        s1_lamrho_min = 0
+        sN_min = 0
+        x_min = np.tile(self.build_params['x_min'], self.build_params['N'] + 1)
+        e_max_min = np.tile(-np.inf, self.build_params['N'] + 1)
+        return u_min, w_min, s1_lamrho_min, sN_min, x_min, e_max_min
+
+    def max_bounds(self):
+        u_max = np.tile(self.build_params['u_max'], self.build_params['N'])
+        # ds_max = np.tile(np.inf, self.build_params['N'])
+        w_max = np.tile(self.build_params['w_max'], self.build_params['N'])
+        s1_lamrho_max = np.inf
+        sN_max = self.build_params['w_max'] * self.build_params['N'] * self.build_params['dt']
+        x_max = np.tile(self.build_params['x_max'], self.build_params['N'] + 1)
+        e_max_max = np.zeros(self.build_params['N'] + 1)
+        return u_max, w_max, s1_lamrho_max, sN_max, x_max, e_max_max
+
+    def sol2state(self, sol, x0, path_pol):
+        x, s, e = self.cs_sol2state(sol, x0, path_pol)
+        # cs.MX to [float]
+        return np.array(x).flatten(), np.array(s).flatten(), np.array(e).flatten()
+
+    def sol2cost(self, sol, x0, path_pol, params, u_prev):
+        cost_params = [params['cs'], params['ce'], params['R'][0], 0, params['DR'][0], 0] + u_prev
+        cost_params[2:6] = [params['R'][0], 0, params['DR'][0], 0]
+        s_cost, e_cost, u1_cost, ud1_cost = self.cs_sol2cost(sol, x0, path_pol, cost_params)
+        cost_params[2:6] = [0, params['R'][1], 0, params['DR'][1]]
+        _, _, u2_cost, ud2_cost = self.cs_sol2cost(sol, x0, path_pol, cost_params)
+        return {'s': float(s_cost), 'e': float(e_cost), 'u': float(u1_cost + u2_cost), 'ud': float(ud1_cost + ud2_cost),
+                'u1': float(u1_cost), 'ud1': float(ud1_cost), 'u2': float(u2_cost), 'ud2': float(ud2_cost)}
+
+    def sol2uds(self, sol):
+        u = sol[:self.build_params['nu'] * self.build_params['N']]
+        ds = sol[self.build_params['nu'] * self.build_params['N']:]
+        return u, ds
+
+    def build(self):
+        # Build parametric optimizer
+        # ------------------------------------
+        params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'],
+                  'path_pol': self.build_params['np'] * (self.build_params['n_pol'] + 1),
+                  'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], 'e_max': 1,
+                  'lam_rho': 1}
+        par_dim = sum(list(params.values()))
+
+        # Exchange parameter dimension with value
+        par = cs.SX.sym("par", par_dim)  # Parameters
+        p_idx = 0
+        for key, dim in params.items():
+            params[key] = par[p_idx:p_idx + dim]
+            p_idx += dim
+
+        # Initialize
+        sol = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N'])
+
+        # Cost
+        cost_params = cs.vertcat(params['cs'], params['ce'], params['R'], params['DR'], params['u_prev'])
+        s_cost, e_cost, u_cost, du_cost = self.cs_sol2cost(sol, params['x0'], params['path_pol'], cost_params)
+        cost = s_cost + e_cost + u_cost + du_cost
+
+        # Constraints
+        x, s, e = self.cs_sol2state(sol, params['x0'], params['path_pol'])
+        u_min, w_min, s1_lamrho_min, sN_min, x_min, e_max_min = self.min_bounds()
+        u_max, w_max, s1_lamrho_max, sN_max, x_max, e_max_max = self.max_bounds()
+        sol_bounds = og.constraints.Rectangle(u_min.tolist() + w_min.tolist(), u_max.tolist() + w_max.tolist())
+        agl_con = cs.vertcat(s[1] - params['lam_rho'],
+                             s,
+                             x,
+                             e - params['e_max'])
+        agl_con_bounds = og.constraints.Rectangle([s1_lamrho_min] + [sN_min] * (self.build_params['N'] + 1) +
+                                                  x_min.tolist() + e_max_min.tolist(),
+                                                  [s1_lamrho_max] + [sN_max] * (self.build_params['N'] + 1) +
+                                                  x_max.tolist() + e_max_max.tolist())
+
+        # Setup builder
+        problem = og.builder.Problem(sol, par, cost) \
+            .with_constraints(sol_bounds) \
+            .with_aug_lagrangian_constraints(agl_con, agl_con_bounds)
+        build_config = og.config.BuildConfiguration() \
+            .with_build_directory(self.build_dir) \
+            .with_build_mode(self.build_params['mode']) \
+            .with_build_python_bindings()
+        meta = og.config.OptimizerMeta() \
+            .with_optimizer_name(self.build_name)
+        solver_config = og.config.SolverConfiguration() \
+            .with_tolerance(self.build_params['solver_tol']) \
+            .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \
+            .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \
+            .with_max_outer_iterations(self.build_params['solver_max_outer_iterations'])
+        builder = og.builder.OpEnOptimizerBuilder(problem,
+                                                  meta,
+                                                  build_config,
+                                                  solver_config) \
+            .with_verbosity_level(1)
+        builder.build()
+        print('')
+
+        # Save build params
+        with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file:
+            yaml.dump(self.build_params, file)
+
+    def shift_sol(self, sol):
+        return sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \
+               sol[self.build_params['nu'] * (self.build_params['N'] - 1) : self.build_params['nu'] * self.build_params['N']] \
+               + sol[self.build_params['nu'] * self.build_params['N']:]
+
+    def run(self, x0, u_prev, path_pol, params, e_max, lam_rho, init_guess=None, verbosity=0):
+
+        p = x0 + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + params['DR'] + \
+            [0.9*e_max, 1.1*lam_rho]
+
+
+        if init_guess is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, 2*e_max, lam_rho):
+            # Use base solution as initial guess
+            init_guess = self.base_solution(x0, path_pol, lam_rho)
+        # Run solver
+        return self.solver.run(p=p, initial_guess=init_guess)
+        solution_data = self.solver.run(p=p, initial_guess=init_guess)
+
+        if solution_data is None or not self.is_feasible(solution_data.solution, x0, path_pol, e_max, lam_rho, d=verbosity>0):
+            sol, exit_status = self.base_solution(x0, path_pol, lam_rho), "BaseSolution"
+        else:
+            sol = solution_data.solution
+            exit_status = solution_data.exit_status
+
+        # self.sol_prev = sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \
+        #                 sol[self.build_params['nu'] * (self.build_params['N'] - 1):self.build_params['nu'] *
+        #                                                                            self.build_params['N']] \
+        #                 + sol[self.build_params['nu'] * self.build_params['N']:]
+        sol_feasible = self.is_feasible(sol, x0, path_pol, e_max, lam_rho, d=verbosity > 0)
+
+        return sol, sol_feasible, exit_status
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/mpc_build/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py
new file mode 100644
index 0000000..d019f27
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/path_generator.py
@@ -0,0 +1,110 @@
+import numpy as np
+from starworlds.utils.misc import tic, toc
+from motion_control.soads.soads import f as soads_f
+
+
+def path_generator(r0, rg, obstacles, workspace, path_s, init_path=None, init_s=None, params=None, verbosity=0):
+    par = {'ds_decay_rate': 0.5, 'ds_increase_rate': 2., 'max_nr_steps': 1000, 'convergence_tolerance': 1e-5,
+           'reactivity': 1., 'crep': 1., 'tail_effect': False, 'max_rhrp_compute_time': np.inf}
+    for k in par.keys():
+        if k in params:
+            par[k] = params[k]
+
+    # Assumes equidistant path_s
+    path_ds = path_s[1] - path_s[0]
+
+    t0 = tic()
+
+    # Initialize
+    ds = path_ds
+    s = np.zeros(par['max_nr_steps'])
+    r = np.zeros((par['max_nr_steps'], r0.size))
+    if init_path is not None:
+        i = init_path.shape[0]
+        r[:i, :] = init_path
+        s[:i] = init_s
+    else:
+        i = 1
+        r[0, :] = r0
+
+
+    L = path_s[-1]
+
+    while True:
+        dist_to_goal = np.linalg.norm(r[i - 1, :] - rg)
+        # Check exit conditions
+        if dist_to_goal < par['convergence_tolerance']:
+            if verbosity > 2:
+                print("[Path Generator]: Path converged. " + str(
+                    int(100 * (s[i - 1] / L))) + "% of path completed.")
+            break
+        if s[i - 1] >= 0.5 * L:
+            if verbosity > 2:
+                print("[Path Generator]: Completed path length. " + str(
+                    int(100 * (s[i - 1] / L))) + "% of path completed.")
+            break
+        if toc(t0) > par['max_rhrp_compute_time']:
+            if verbosity > 0:
+                print("[Path Generator]: Max compute time in path integrator. " + str(
+                    int(100 * (s[i - 1] / L))) + "% of path completed.")
+            break
+        if i >= par['max_nr_steps']:
+            if verbosity > 0:
+                print("[Path Generator]: Max steps taken in path integrator. " + str(
+                    int(100 * (s[i - 1] / L))) + "% of path completed.")
+            break
+
+        # Movement using SOADS dynamics
+        dr = min(1, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, workspace=workspace, adapt_obstacle_velocity=False,
+                                            unit_magnitude=True, crep=par['crep'],
+                                            reactivity=par['reactivity'], tail_effect=par['tail_effect'],
+                                            convergence_tolerance=par['convergence_tolerance'])
+        r[i, :] = r[i - 1, :] + dr * ds
+        ri_collision = False
+
+        def collision_free(p):
+            return all([o.exterior_point(p) for o in obstacles]) and workspace.interior_point(p)
+
+        # Adjust for collisions that may arise due to crep!=1
+        if not collision_free(r[i, :]):
+            dr = min(1, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, workspace=workspace,
+                                                adapt_obstacle_velocity=False,
+                                                unit_magnitude=True, crep=1,
+                                                reactivity=1, convergence_tolerance=par['convergence_tolerance'])
+            r[i, :] = r[i - 1, :] + dr * ds
+
+            # while any([o.interior_point(r[i, :]) for o in obstacles]):
+            while not collision_free(r[i, :]):
+                if verbosity > 2:
+                    print("[Path Generator]: Path inside obstacle. Reducing integration step from {:5f} to {:5f}.".format(ds, ds*par['ds_decay_rate']))
+                ds *= par['ds_decay_rate']
+                r[i, :] = r[i - 1, :] + dr * ds
+                # if ds < 0.01:
+                #     import matplotlib.pyplot as plt
+                #     plt.gca().quiver(*r[i-1, :], *dr, color='g')
+                #     for o in obstacles:
+                #         plt.gca().quiver(*o.boundary_mapping(r[i-1, :]), *o.normal(r[i-1, :]))
+                #     plt.show()
+
+                # Additional compute time check
+                if toc(t0) > par['max_rhrp_compute_time']:
+                    ri_collision = True
+                    break
+        if ri_collision:
+            continue
+
+        # Update travelled distance
+        s[i] = s[i - 1] + ds
+        # Try to increase step rate again
+        ds = min(par['ds_increase_rate'] * ds, path_ds)
+        # Increase iteration counter
+        i += 1
+
+    r = r[:i, :]
+    s = s[:i]
+
+    # Evenly spaced path
+    path = np.vstack((np.interp(path_s, s, r[:, 0]), np.interp(path_s, s, r[:, 1]))).T
+
+    compute_time = toc(t0)
+    return path, s[-1], compute_time
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py
new file mode 100644
index 0000000..24c8a4b
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_ds/workspace_modification.py
@@ -0,0 +1,139 @@
+import numpy as np
+from starworlds.starshaped_hull import cluster_and_starify, ObstacleCluster
+from starworlds.utils.misc import tic, toc
+import shapely
+
+def rho_environment(workspace, obstacles, rho):
+    obstacles_rho = [o.dilated_obstacle(padding=rho, id="duplicate") for o in obstacles]
+    workspace_rho = workspace.dilated_obstacle(padding=-rho, id="duplicate")
+    obstacles_rho_sh = shapely.ops.unary_union([o.polygon() for o in obstacles_rho])
+    free_rho_sh = workspace_rho.polygon().difference(obstacles_rho_sh)
+    return obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh
+
+
+def extract_r0(r_plus, p, rho, free_rho_sh, workspace_rho, obstacles_rho):
+    initial_reference_set = shapely.geometry.Point(p).buffer(rho).intersection(free_rho_sh.buffer(-1e-2))
+    if initial_reference_set.is_empty:
+        if rho < 1e-5:
+            print(rho, free_rho_sh.exterior.distance(shapely.geometry.Point(p)))
+            import matplotlib.pyplot as plt
+            from starworlds.utils.misc import draw_shapely_polygon
+            _, ax = plt.subplots()
+            draw_shapely_polygon(free_rho_sh.buffer(-1e-2), ax=ax, fc='g')
+            ax.plot(*r_plus, 'rs')
+            ax.plot(*p, 'ks')
+            ax.plot(*shapely.geometry.Point(p).buffer(rho).exterior.xy, 'b--')
+            for o in obstacles_rho:
+                o.draw(ax=ax, fc='r', alpha=0.3)
+                # ax.plot(*o.polygon().exterior.xy, 'k--')
+            # for o in obstacles:
+            #     o.draw(ax=ax, fc='lightgrey')
+            plt.show()
+
+        return None
+    r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(r_plus))
+    r0 = np.array(r0_sh.coords[0])
+    if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]):
+        # initial_reference_set = initial_reference_set.buffer(-rho_buffer)
+        r0_sh = initial_reference_set.centroid
+        r0 = np.array(r0_sh.coords[0])
+    if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]):
+        return None
+    return r0
+
+def workspace_modification(obstacles, workspace, p, pg, r_plus, rho0, hull_epsilon, previous_obstacle_clusters=None, params=None, verbosity=0):
+    par = {'max_obs_compute_time': np.inf, 'gamma': 0.5, 'make_convex': 1, 'iterative_rho_reduction': 1,
+          'use_previous_workspace': 1}
+    for k in par.keys():
+        if k in params:
+            par[k] = params[k]
+
+    p_sh = shapely.geometry.Point(p)
+    # Clearance variable initialization
+    t_init = tic()
+    rho = rho0
+
+    # Pad obstacles with rho
+    obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh = rho_environment(workspace, obstacles, rho)
+    r0 = extract_r0(r_plus, p, rho, free_rho_sh, workspace_rho, obstacles_rho)
+    while r0 is None:
+        if par['iterative_rho_reduction'] or rho < rho0:
+            rho *= par['gamma']
+        else:
+            print(min([o.polygon().distance(p_sh) for o in obstacles] + [workspace_rho.polygon().exterior.distance(p_sh)]))
+            obstacles_dist = min([o.polygon().distance(p_sh) for o in obstacles] + [workspace_rho.polygon().exterior.distance(p_sh)])
+            rho = 0.8 * obstacles_dist
+        obstacles_rho, workspace_rho, free_rho_sh, obstacles_rho_sh = rho_environment(workspace, obstacles, rho)
+        r0 = extract_r0(r_plus, p, rho, free_rho_sh, workspace_rho, obstacles_rho)
+        if verbosity > 1:
+            print("[Workspace modification]: Reducing rho to " + str(rho))
+
+
+
+        # r0 = extract_r0(initial_reference_set, r_plus, workspace_rho, obstacles_rho)
+
+        # initial_reference_set = initial_reference_set.buffer(-0.2 * initial_reference_set.minimum_clearance)
+
+        # Initial reference position selection
+        # r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(r_plus))
+        # r0 = np.array(r0_sh.coords[0])
+        # if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]):
+        #     # initial_reference_set = initial_reference_set.buffer(-rho_buffer)
+        #     r0_sh = initial_reference_set.centroid
+        #     r0 = np.array(r0_sh.coords[0])
+        #
+        # if not all([o.exterior_point(r0) for o in obstacles_rho] + [workspace_rho.interior_point(r0)]):
+        #     if verbosity > 1:
+        #         print("[Workspace Modification]: r0 not valid. Reducing rho to " + str(rho * par['gamma']))
+
+
+
+    # Goal reference position selection
+    rg_sh = shapely.geometry.Point(pg)
+    rg = pg
+    rho_buffer = 0
+    while not all([o.exterior_point(rg) for o in obstacles_rho] + [workspace_rho.interior_point(rg)]):
+        rg_sh, _ = shapely.ops.nearest_points(workspace_rho.polygon().buffer(-rho_buffer).difference(obstacles_rho_sh.buffer(rho_buffer)), shapely.geometry.Point(pg))
+    # while not all([o.exterior_point(rg) for o in obstacles_rho]):
+    #     rg_sh, _ = shapely.ops.nearest_points(shapely.geometry.box(-1e6,-1e6,1e6,1e6).difference(obstacles_rho_sh.buffer(rho_buffer)), shapely.geometry.Point(pg))
+        rg = np.array(rg_sh.coords[0])
+        rho_buffer += 1e-2
+
+    # TODO: Check more thoroughly
+    if par['use_previous_workspace'] and previous_obstacle_clusters is not None:
+        obstacles_star_sh = shapely.ops.unary_union([o.polygon() for o in previous_obstacle_clusters]).buffer(1e-4)
+        if obstacles_star_sh.disjoint(shapely.geometry.Point(r0)) and obstacles_star_sh.disjoint(rg_sh) and obstacles_rho_sh.within(obstacles_star_sh):
+            if verbosity > 2:
+                print("[Workspace modification]: Reuse workspace from previous time step.")
+            obstacle_clusters = previous_obstacle_clusters
+            exit_flag = 10
+            compute_time = toc(t_init)
+            obstacle_timing = None
+            return obstacle_clusters, r0, rg, rho, obstacles_rho, workspace_rho, compute_time, obstacle_timing, exit_flag
+        else:
+            if verbosity > 2:
+                print("[Workspace modification]: No reuse workspace from previous time step.")
+                print(obstacles_star_sh.disjoint(shapely.geometry.Point(r0)), obstacles_star_sh.disjoint(rg_sh), obstacles_rho_sh.within(obstacles_star_sh))
+    else:
+        if par['use_previous_workspace'] and verbosity > 2:
+            print("[Workspace modification]: No reuse workspace from previous time step.")
+                # import matplotlib.pyplot as plt
+                # from starworlds.utils.misc import draw_shapely_polygon
+                # _,ax = plt.subplots()
+                # ax.plot([obstacles_rho_sh.bounds[0], obstacles_rho_sh.bounds[2]],
+                #         [obstacles_rho_sh.bounds[1], obstacles_rho_sh.bounds[3]], alpha=0)
+                # draw_shapely_polygon(obstacles_star_sh, ax=ax, fc='g', alpha=0.7)
+                # draw_shapely_polygon(obstacles_rho_sh.difference(obstacles_star_sh), ax=ax, fc='r', alpha=1, ec='k', linestyle='--')
+                # plt.show()
+
+
+    # Apply cluster and starify
+    obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles_rho, r0, rg, hull_epsilon,
+                                                                                workspace=workspace_rho,
+                                                                                max_compute_time=par['max_obs_compute_time']-toc(t_init),
+                                                                                previous_clusters=previous_obstacle_clusters,
+                                                                                make_convex=par['make_convex'], verbose=verbosity,
+                                                                                timing_verbose=0)
+
+    compute_time = toc(t_init)
+    return obstacle_clusters, r0, rg, rho, obstacles_rho, workspace_rho, compute_time, obstacle_timing, exit_flag
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py
new file mode 100644
index 0000000..987aeaa
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/__init__.py
@@ -0,0 +1,2 @@
+from .mpc import Mpc, pol2pos
+from .motion_controller import MotionController
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py
new file mode 100644
index 0000000..f51976a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/motion_controller.py
@@ -0,0 +1,138 @@
+from motion_control.pfmpc_obstacle_constraints import Mpc, pol2pos
+from starworlds.utils.misc import tic, toc
+import numpy as np
+import shapely
+
+
+class MotionController:
+    def __init__(self, params, robot, reference_path, verbosity=0):
+        self.params = params
+        self.robot = robot
+        self.mpc = Mpc(params, robot)
+        self.verbosity = verbosity
+        self.reference_path = shapely.geometry.Point(reference_path[0]) if len(reference_path) == 1 else shapely.geometry.LineString(reference_path)  # Waypoints
+        self.theta_g = self.reference_path.length
+        self.rhrp_L = self.params['N'] * self.params['dt'] * self.mpc.build_params['w_max']
+        self.rhrp_s = np.linspace(0, self.rhrp_L, params['rhrp_steps'])
+        self.reset()
+
+    def reset(self):
+        self.mpc.reset()
+        self.r_plus = None
+        self.theta = 0
+        self.rhrp_path = None
+        # self.rhrp_L = self.params['N'] * self.mpc.build_params['dp_max']
+        # self.rhrp_s = np.arange(0, self.rhrp_L + self.rhrp_ds, self.rhrp_ds)
+        self.path_pol = None
+        self.epsilon = None
+        self.solution = None
+        self.sol_feasible = None
+        self.u_prev = [0] * self.robot.nu
+        self.timing = {'workspace': 0, 'target': 0, 'mpc': 0}
+
+    def extract_obs_par(self, obstacles):
+        n_ell_par = 3 * self.mpc.build_params['max_No_ell'] * (1 + self.mpc.build_params['N_obs_predict'])
+        n_pol_par = self.mpc.build_params['max_No_pol'] * self.mpc.build_params['max_No_vert'] * 2
+        obs_par = [0] * (n_ell_par + n_pol_par)
+        obs_par[1:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params[
+            'max_No_ell']
+        obs_par[2:n_ell_par:3 * (1 + self.mpc.build_params['N_obs_predict'])] = [1] * self.mpc.build_params[
+            'max_No_ell']
+        obs_par_padded = obs_par.copy()
+        n_ell, n_pol = 0, 0
+
+        for o in obstacles:
+            if hasattr(o, "_a"):
+                # print(o.pos())
+                j = n_ell * 3 * (1 + self.mpc.build_params['N_obs_predict'])
+                obs_par[j] = 1  # Include obstacle
+                obs_par[j + 1:j + 3] = o._a  # Ellipse axes
+                mm = o._motion_model
+                pos, rot, t = mm.pos().copy(), mm.rot(), mm._t
+                if hasattr(mm, '_wp_idx'):
+                    wp_idx = mm._wp_idx
+                for k in range(self.mpc.build_params['N_obs_predict']):
+                    obs_par[j + 3 + 3 * k:j + 5 + 3 * k] = mm.pos()  # Ellipse position
+                    obs_par[j + 5 + 3 * k] = mm.rot()  # Ellipse orientation
+                    mm.move(None, self.mpc.build_params['dt'])
+                mm.set_pos(pos), mm.set_rot(rot)
+                mm._t = t
+                if hasattr(mm, '_wp_idx'):
+                    mm._wp_idx = wp_idx
+
+                obs_par_padded[j:j + 3 * (self.mpc.build_params['N_obs_predict'] + 1)] = obs_par[j:j + 3 * (
+                            self.mpc.build_params['N_obs_predict'] + 1)]
+                obs_par_padded[j + 1:j + 3] = o._a + self.params['obstacle_padding']
+
+                n_ell += 1
+
+            if hasattr(o, "vertices"):
+                idx = n_ell_par + n_pol * self.mpc.build_params['max_No_vert'] * 2
+                vertices = shapely.ops.orient(o.polygon()).exterior.coords[:-1]
+                for i in range(self.mpc.build_params['max_No_vert']):
+                    obs_par[idx + i * 2:idx + (i + 1) * 2] = vertices[i % len(vertices)]
+                vertices = shapely.ops.orient(
+                    o.polygon().buffer(self.params['obstacle_padding'], quad_segs=1, cap_style=3,
+                                       join_style=2)).exterior.coords[:-1]
+                for i in range(self.mpc.build_params['max_No_vert']):
+                    obs_par_padded[idx + i * 2:idx + (i + 1) * 2] = vertices[i % len(vertices)]
+                n_pol += 1
+        return obs_par, obs_par_padded
+
+    def compute_u(self, x):
+        return self.u_prev
+
+    def update_policy(self, x, obstacles, workspace=None):
+        p = self.robot.h(x)
+
+        # Initialize r_plus to robot position
+        if self.r_plus is None:
+            self.r_plus = p
+
+        # Extract receding path from global target path
+        t0 = tic()
+        rhrp_path_sh = shapely.ops.substring(self.reference_path, start_dist=self.theta, end_dist=self.theta + self.rhrp_L)
+        if rhrp_path_sh.length > 0:
+            self.rhrp_path = np.array([rhrp_path_sh.interpolate(s).coords[0] for s in self.rhrp_s])
+        else:
+            self.rhrp_path = np.tile(rhrp_path_sh.coords[0], (len(self.rhrp_s), 1))
+
+        # Approximate target with polynomials
+        self.path_pol = np.polyfit(self.rhrp_s, self.rhrp_path[:, 0], self.params['n_pol']).tolist() + \
+                        np.polyfit(self.rhrp_s, self.rhrp_path[:, 1], self.params['n_pol']).tolist()
+        # Force init position to be correct
+        self.path_pol[self.params['n_pol']] = self.rhrp_path[0, 0]
+        self.path_pol[-1] = self.rhrp_path[0, 1]
+        # Compute polyfit approximation error
+        self.epsilon = max(np.linalg.norm(self.rhrp_path - np.array([pol2pos(self.path_pol, s, self.mpc.build_params['n_pol']) for s in self.rhrp_s]), axis=1))
+        self.timing['target'] = toc(t0)
+
+        t0 = tic()
+        # Extract obstacle parameters (Assumes all ellipses)
+        obs_par, obs_par_padded = self.extract_obs_par(obstacles)
+        # Compute MPC solution
+        if self.solution is not None:
+            init_guess = self.solution.copy()
+        else:
+            init_guess = None
+        solution_data = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, obs_par_padded, init_guess)
+        if solution_data is None:
+            self.sol_feasible, self.mpc_exit_status = False, "None"
+        else:
+            self.solution, self.mpc_exit_status = solution_data.solution, solution_data.exit_status
+            self.sol_feasible = self.mpc.is_feasible(self.solution, x.tolist(), self.path_pol, obs_par, d=self.verbosity > 0)
+
+        if self.sol_feasible:
+            u, w = self.mpc.sol2uds(self.solution)
+            self.u_prev = u[:self.robot.nu]
+
+            p_ref_dist = rhrp_path_sh.distance(shapely.geometry.Point(p))
+            if self.theta_g > 0 and p_ref_dist < 1:
+                self.theta = min(self.theta + w[0] * self.params['dt'], self.theta_g)
+        else:
+            self.u_prev = [0] * self.robot.nu
+
+        self.timing['mpc'] = toc(t0)
+
+
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py
new file mode 100644
index 0000000..917d47a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc.py
@@ -0,0 +1,398 @@
+import casadi.casadi as cs
+import opengen as og
+import numpy as np
+import os, sys
+import yaml
+
+
+class NoSolutionError(Exception):
+    '''raise this when there's no solution to the mpc problem'''
+    pass
+
+def pol_eval(pol_coeff, x, n_pol):
+    return sum([pol_coeff[i] * x ** (n_pol - i) for i in range(n_pol+1)])
+
+
+def pol2pos(pol_coeff, s, n_pol):
+    return [pol_eval(pol_coeff[:n_pol+1], s, n_pol), pol_eval(pol_coeff[n_pol+1:], s, n_pol)]
+
+# Returns >0 for interior points, <0 for exterior points, =0 for boundary points
+def ellipse_function(p, pos, rot, ax):
+    x_dist = p[0] - pos[0]
+    y_dist = p[1] - pos[1]
+    s, c = cs.sin(rot), cs.cos(rot)
+    normalized_center_dist = ((x_dist * c + y_dist * s) ** 2) / (ax[0] ** 2) + ((x_dist * s + y_dist * c) ** 2) / (ax[1] ** 2)
+    return 1 - normalized_center_dist
+
+# Returns >0 for interior points, otherwise 0
+def convex_polygon_function(p, vertices, N):
+    hp_vals = []
+    for i in range(N-1):
+        idc1, idc2 = i*2, (i+1) % N * 2
+        x1, y1 = vertices[idc1], vertices[idc1+1]
+        x2, y2 = vertices[idc2], vertices[idc2+1]
+        dx = x2 - x1
+        dy = y2 - y1
+        Ai = cs.vertcat(dy, -dx)
+        bi = x1 * dy - y1 * dx
+        hp_vals += [bi-cs.dot(Ai, p)]
+    return cs.vertcat(*hp_vals)
+
+class Mpc:
+
+    def __init__(self, params, robot):
+        self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build')
+        # Load parameters
+        self.build_params = None
+        self.robot = robot
+        self.set_build_params(params)
+        rebuild, self.build_name = self.get_build_version()
+        self.generate_sol_fun()
+        # Build if different from latest build
+        if rebuild:
+            self.build()
+        else:
+            print("Found MPC build: {}".format(self.build_name))
+
+        sys.path.insert(1, os.path.join(self.build_dir, self.build_name))
+        optimizer = __import__(self.build_name)
+        self.solver = optimizer.solver()
+        self.reset()
+
+    def reset(self):
+        self.sol_prev = None
+
+    def set_build_params(self, params):
+        self.build_params = {
+            'mode': params['build_mode'],
+            'N': params['N'],
+            'dt': params['dt'],
+            'solver_tol': params['solver_tol'],
+            'solver_max_time': params['solver_max_time'],
+            'solver_max_inner_iterations': params['solver_max_inner_iterations'],
+            'solver_max_outer_iterations': params['solver_max_outer_iterations'],
+            'x_min': self.robot.x_min,
+            'x_max': self.robot.x_max,
+            'u_min': self.robot.u_min,
+            'u_max': self.robot.u_max,
+            'nx': self.robot.nx,
+            'nu': self.robot.nu,
+            'np': 2,
+            'robot_model': self.robot.__class__.__name__,
+            'n_pol': params['n_pol'],
+            'integration_method': params['integration_method'],
+            'w_max': self.robot.vmax,
+            'max_No_ell': params['max_No_ell'],
+            'max_No_pol': params['max_No_pol'],
+            'max_No_vert': params['max_No_vert'],
+            'N_obs_predict': params['N_obs_predict'],
+        }
+
+    def get_build_version(self):
+        builds = [name for name in os.listdir(self.build_dir)
+                  if os.path.isdir(os.path.join(self.build_dir, name))]
+        for build_ver in builds:
+            if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')):
+                continue
+            with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file:
+                build_ver_params = yaml.load(file, Loader=yaml.FullLoader)
+            if self.build_params == build_ver_params:
+                return False, build_ver
+        ver = 0
+        while 'ver' + str(ver) in os.listdir(self.build_dir):
+            ver += 1
+        return True, 'ver' + str(ver)
+
+    def cs_obstacle_evaluation(self, obs_par, x, k):
+        n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict'])
+        obs_val = []
+        for i in range(self.build_params['max_No_ell']):
+            j = i * 3 * (1 + self.build_params['N_obs_predict'])
+            time_idx = min(k, self.build_params['N_obs_predict']-1)
+            include_obs = obs_par[j]
+            ell_axs = obs_par[j + 1:j + 3]
+            ell_pos = obs_par[j + 3 + 3 * time_idx:j + 5 + 3 * time_idx]
+            ell_rot = obs_par[j + 5 + 3 * time_idx]
+            ell_val = ellipse_function(self.robot.h(x), ell_pos, ell_rot, ell_axs)
+            obs_val += [cs.fmax(include_obs * ell_val, 0)]
+
+        for i in range(self.build_params['max_No_pol']):
+            j = n_ell_par + i * self.build_params['max_No_vert'] * 2
+            hp_vals = convex_polygon_function(self.robot.h(x), obs_par[j : j + 2 * self.build_params['max_No_vert']], self.build_params['max_No_vert'])
+            obs_val += [cs.fmax(cs.mmin(hp_vals),0)]
+        return cs.vertcat(*obs_val)
+
+    def is_feasible(self, sol, x0, path_pol, obs_par, d=False):
+        u, w = self.sol2uds(sol)
+        x, s, e = self.sol2state(sol, x0, path_pol)
+        u_min, w_min, sN_min, x_min, obs_val_min = self.min_bounds()
+        u_max, w_max, sN_max,  x_max, obs_val_max = self.max_bounds()
+        obs_val = self.sol2obsval(sol, x0, obs_par)
+
+        eps_val = 1e-8
+        def in_range(val, min, max):
+            return ((val >= min - eps_val) & (val <= max + eps_val)).all()
+
+        sol_ok = True
+        if not in_range(u, u_min, u_max):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad u")
+                print(u)
+        if not in_range(w, w_min, w_max):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad w")
+                print(w)
+        if not in_range(x, x_min, x_max):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad x")
+                print(x)
+        if not in_range(s[-1], sN_min, sN_max):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad sN {:.4f} > {:.4f}".format(s[-1], sN_max))
+        if not in_range(obs_val, 0, 0):
+            sol_ok = False
+        return sol_ok
+
+    def error(self, x, s, path_pol):
+        p_ref = cs.vertcat(*pol2pos(path_pol, s, self.build_params['n_pol']))
+        return cs.norm_2(p_ref - self.robot.h(x))
+
+    def base_solution(self, x0, path_pol, w0):
+        u = [0] * (self.build_params['N'] * self.build_params['nu'])
+        w = [0] * (self.build_params['N'])
+        w[0] = w0
+
+        if self.build_params['robot_model'] == 'Unicycle':
+            p_ref = pol2pos(path_pol, w0*self.build_params['dt'], self.build_params['n_pol'])
+            k1, k2 = 10, 2
+            # k1, k2 = 0.15, 0.3
+
+            x = x0.copy()
+            for i in range(self.build_params['N']):
+                e = np.array(x[:2]) - np.array(p_ref)
+                th = x[2]
+
+                r_th = np.arctan2(e[1], e[0]) + np.pi
+                # E Theta in (-pi, pi]
+                e_th = r_th - th
+                e_th0 = e_th
+                while e_th > np.pi:
+                    e_th -= 2 * np.pi
+                while e_th <= -np.pi:
+                    e_th += 2 * np.pi
+
+                ui = [
+                    -k1 * (e[0] * np.cos(th) + e[1] * np.sin(th)),
+                    k2 * (e_th)
+                ]
+
+                if self.robot.u_min[0] == 0 and ui[0] < 0:
+                    ui[0] = 0
+                    u0_sig = 1
+                else:
+                    u0_sig = ui[0] / self.robot.u_max[0] if ui[0] >= 0 else ui[0] / self.robot.u_min[0]
+                u1_sig = ui[1] / self.robot.u_max[1] if ui[1] >= 0 else ui[1] / self.robot.u_min[1]
+
+                sig = max(u0_sig, u1_sig, 1)
+                if sig > 1:
+                    if u0_sig > u1_sig:
+                        ui[0] = self.robot.u_max[0] if ui[0] > 0 else self.robot.u_min[0]
+                        ui[1] /= sig
+                    else:
+                        ui[0] /= sig
+                        ui[1] = self.robot.u_max[1] if ui[1] > 0 else self.robot.u_min[1]
+                u[i * self.build_params['nu']:(i+1) * self.build_params['nu']] = ui
+
+                x, _ = self.robot.move(x, ui, self.build_params['dt'])
+
+        return u + w
+
+    def discrete_integration(self, f, x, u):
+        dt = self.build_params['dt']
+        if self.build_params['integration_method'] == 'euler':
+            return x + f(x, u) * dt
+        if self.build_params['integration_method'] == 'RK4':
+            k1 = f(x, u)
+            k2 = f(x + dt / 2 * k1, u)
+            k3 = f(x + dt / 2 * k2, u)
+            k4 = f(x + dt * k3, u)
+            return x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
+        raise NotImplemented
+
+    def generate_sol_fun(self):
+        # Initialize
+        sol_sym = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N'])
+        x0_sym = cs.SX.sym('x0', self.build_params['nx'])
+        path_pol_sym = cs.SX.sym('path_pol', self.build_params['np'] * (self.build_params['n_pol'] + 1))
+        n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict'])
+        n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2
+        obs_par_sym = cs.SX.sym('obs_par', n_ell_par + n_pol_par)
+        cost_params = {'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], 'obs_pen': 1,
+                       'u_prev': self.build_params['nu']}
+        cost_params_sym = cs.SX.sym("cost_params", sum(list(cost_params.values())))
+        # Exchange parameter dimension with SX variable
+        p_idx = 0
+        for key, dim in cost_params.items():
+            cost_params[key] = cost_params_sym[p_idx:p_idx + dim]
+            p_idx += dim
+        # Initialize
+        u, w = self.sol2uds(sol_sym)
+        x_k, s_k = x0_sym, 0
+        e_k = self.error(x_k, s_k, path_pol_sym)
+        obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, 0)
+        x, s, e, obs_val = x_k, s_k, e_k, obs_val_k
+        # Loop over time steps
+        for k in range(self.build_params['N']):
+            # Current control variables
+            u_k = u[k * self.build_params['nu']:(k + 1) * self.build_params['nu']]
+            # Integrate one step
+            x_k = self.discrete_integration(lambda x, u: cs.vertcat(*self.robot.f(x,u)), x_k, u_k)
+            s_k = self.discrete_integration(lambda s, w: w, s_k, w[k])
+            # Extract error and obstacle values
+            e_k = self.error(x_k, s_k, path_pol_sym)
+            obs_val_k = self.cs_obstacle_evaluation(obs_par_sym, x_k, k+1)
+            # Store current state
+            x = cs.vertcat(x, x_k)
+            s = cs.vertcat(s, s_k)
+            e = cs.vertcat(e, e_k)
+            obs_val = cs.vertcat(obs_val, obs_val_k)
+        # Define costs
+        u_target = cs.SX([self.build_params['w_max'], 0])
+        du = u - cs.vertcat(cost_params['u_prev'], u[:-self.build_params['nu']])
+        u_err = cs.repmat(u_target,self.build_params['N']) - u
+        R = cs.repmat(cost_params['R'], self.build_params['N'])
+        DR = cs.repmat(cost_params['DR'], self.build_params['N'])
+        w_cost = cost_params['cs'] * cs.sum1(self.build_params['w_max'] - w)
+        e_cost = cost_params['ce'] * cs.sum1(e)
+        u_cost = cs.dot(R, u_err * u_err)
+        du_cost = cs.dot(DR, du * du)
+        obs_cost = cost_params['obs_pen'] * cs.sum1(obs_val * obs_val)
+        # Define constraints
+        self.cs_sol2state = cs.Function('cs_sol2state', [sol_sym, x0_sym, path_pol_sym], [x, s, e], ['sol', 'x0', 'path_pol'], ['x', 's', 'e'])
+        self.cs_sol2cost = cs.Function('cs_sol2cost', [sol_sym, x0_sym, path_pol_sym, obs_par_sym, cost_params_sym], [w_cost, e_cost, u_cost, du_cost, obs_cost], ['sol', 'x0', 'path_pol', 'obs_par', 'cost_params'], ['s_cost', 'e_cost', 'u_cost', 'du_cost', 'obs_cost'])
+        self.cs_sol2obsval = cs.Function('cs_sol2obsval', [sol_sym, x0_sym, obs_par_sym], [obs_val], ['sol', 'x0', 'obs_par'], ['obs_val'])
+
+    def min_bounds(self):
+        u_min = np.tile(self.build_params['u_min'], self.build_params['N'])
+        w_min = np.zeros(self.build_params['N'])
+        sN_min = 0
+        x_min = np.tile(self.build_params['x_min'], self.build_params['N'] + 1)
+        obs_val_min = np.tile(-np.inf, self.build_params['max_No_ell'] * (self.build_params['N'] + 1))
+        return u_min, w_min, sN_min, x_min, obs_val_min
+
+    def max_bounds(self):
+        u_max = np.tile(self.build_params['u_max'], self.build_params['N'])
+        w_max = np.tile(self.build_params['w_max'], self.build_params['N'])
+        sN_max = self.build_params['w_max'] * self.build_params['N'] * self.build_params['dt']
+        x_max = np.tile(self.build_params['x_max'], self.build_params['N'] + 1)
+        obs_val_max = np.zeros(self.build_params['max_No_ell'] * (self.build_params['N'] + 1))
+        return u_max, w_max, sN_max, x_max, obs_val_max
+
+    def sol2state(self, sol, x0, path_pol):
+        x, s, e = self.cs_sol2state(sol, x0, path_pol)
+        # cs.MX to [float]
+        return np.array(x).flatten(), np.array(s).flatten(), np.array(e).flatten()
+
+    def sol2obsval(self, sol, x0, obs_par):
+        return np.array(self.cs_sol2obsval(sol, x0, obs_par)).flatten()
+
+    def sol2cost(self, sol, x0, path_pol, obs_par, params, u_prev):
+        cost_params = [params['cs'], params['ce'], params['R'][0], 0, params['DR'][0], 0, params['obs_pen']] + u_prev
+        cost_params[2:6] = [params['R'][0], 0, params['DR'][0], 0]
+        s_cost, e_cost, u1_cost, ud1_cost, obs_cost = self.cs_sol2cost(sol, x0, path_pol, obs_par, cost_params)
+        cost_params[2:6] = [0, params['R'][1], 0, params['DR'][1]]
+        _, _, u2_cost, ud2_cost, _ = self.cs_sol2cost(sol, x0, path_pol, obs_par, cost_params)
+        return {'s': float(s_cost), 'e': float(e_cost), 'u': float(u1_cost + u2_cost), 'ud': float(ud1_cost + ud2_cost),
+                'u1': float(u1_cost), 'ud1': float(ud1_cost), 'u2': float(u2_cost), 'ud2': float(ud2_cost), 'obs': float(obs_cost)}
+
+    def sol2uds(self, sol):
+        u = sol[:self.build_params['nu'] * self.build_params['N']]
+        ds = sol[self.build_params['nu'] * self.build_params['N']:]
+        return u, ds
+
+    def build(self):
+        # Build parametric optimizer
+        # ------------------------------------
+        n_ell_par = 3 * self.build_params['max_No_ell'] * (1 + self.build_params['N_obs_predict'])
+        n_pol_par = self.build_params['max_No_pol'] * self.build_params['max_No_vert'] * 2
+
+        params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'],
+                  'path_pol': self.build_params['np'] * (self.build_params['n_pol'] + 1),
+                  'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'DR': self.build_params['nu'], 'obs_pen': 1,
+                  'obs_par': n_ell_par + n_pol_par}
+        par_dim = sum(list(params.values()))
+
+        # Exchange parameter dimension with value
+        par = cs.SX.sym("par", par_dim)  # Parameters
+        p_idx = 0
+        for key, dim in params.items():
+            params[key] = par[p_idx:p_idx + dim]
+            p_idx += dim
+
+        # Initialize
+        sol = cs.SX.sym('sol', (self.build_params['nu'] + 1) * self.build_params['N'])
+
+        # Cost
+        cost_params = cs.vertcat(params['cs'], params['ce'], params['R'], params['DR'], params['obs_pen'], params['u_prev'])
+        s_cost, e_cost, u_cost, du_cost, obs_cost = self.cs_sol2cost(sol, params['x0'], params['path_pol'], params['obs_par'], cost_params)
+        cost = s_cost + e_cost + u_cost + du_cost + obs_cost
+
+        # Constraints
+        x, s, e = self.cs_sol2state(sol, params['x0'], params['path_pol'])
+        obs_val = self.cs_sol2obsval(sol, params['x0'], params['obs_par'])
+        u_min, w_min, sN_min, x_min, obs_val_min = self.min_bounds()
+        u_max, w_max, sN_max, x_max, obs_val_max = self.max_bounds()
+        sol_bounds = og.constraints.Rectangle(u_min.tolist() + w_min.tolist(), u_max.tolist() + w_max.tolist())
+        agl_con = cs.vertcat(s,
+                             x,
+                             obs_val)
+        agl_con_bounds = og.constraints.Rectangle([sN_min] * (self.build_params['N'] + 1) +
+                                                  x_min.tolist() + obs_val_min.tolist(),
+                                                  [sN_max] * (self.build_params['N'] + 1) +
+                                                  x_max.tolist() + obs_val_max.tolist())
+
+        # Setup builder
+        problem = og.builder.Problem(sol, par, cost) \
+            .with_constraints(sol_bounds) \
+            .with_aug_lagrangian_constraints(agl_con, agl_con_bounds)
+        build_config = og.config.BuildConfiguration() \
+            .with_build_directory(self.build_dir) \
+            .with_build_mode(self.build_params['mode']) \
+            .with_build_python_bindings()
+        meta = og.config.OptimizerMeta() \
+            .with_optimizer_name(self.build_name)
+        solver_config = og.config.SolverConfiguration() \
+            .with_tolerance(self.build_params['solver_tol']) \
+            .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \
+            .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \
+            .with_max_outer_iterations(self.build_params['solver_max_outer_iterations'])
+        builder = og.builder.OpEnOptimizerBuilder(problem,
+                                                  meta,
+                                                  build_config,
+                                                  solver_config) \
+            .with_verbosity_level(1)
+        builder.build()
+        print('')
+
+        # Save build params
+        with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file:
+            yaml.dump(self.build_params, file)
+
+    def shift_sol(self, sol):
+        return sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \
+               sol[self.build_params['nu'] * (self.build_params['N'] - 1) : self.build_params['nu'] * self.build_params['N']] \
+               + sol[self.build_params['nu'] * self.build_params['N']:]
+
+    def run(self, x0, u_prev, path_pol, params, obs_par, init_guess=None):
+        p = x0 + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + params['DR'] + [params['obs_pen']] + obs_par
+
+        if init_guess is None:# or not self.is_feasible(self.sol_prev, x0, path_pol, 2*e_max, lam_rho):
+            # Use base solution as initial guess
+            init_guess = self.base_solution(x0, path_pol, 0.1)
+        # Run solver
+        return self.solver.run(p=p, initial_guess=init_guess)
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py
new file mode 100644
index 0000000..e6fff9e
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/__init__.py
@@ -0,0 +1 @@
+from .soads import f, SoadsController, draw_vector_field, compute_weights, f_nominal, rollout
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py
new file mode 100644
index 0000000..6a1c11a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/motion_control/soads/soads.py
@@ -0,0 +1,386 @@
+import numpy as np
+import shapely
+from starworlds.starshaped_hull import cluster_and_starify
+from starworlds.utils.misc import tic, toc
+
+class SoadsController:
+
+    def __init__(self, params, robot, verbose=False):
+        if not robot.__class__.__name__ == 'Omnidirectional':
+            raise NotImplementedError("SoadsController only implemented for Omnidirectional robots.")
+        self.params = params
+        self.robot = robot
+        self.obstacle_clusters = None
+        self.obstacles_star = []
+        self.dp_prev = None
+        self.verbose = verbose
+        self.timing = {'obstacle': 0, 'control': 0}
+
+    def compute_u(self, x, pg, obstacles, workspace=None):
+        p = self.robot.h(x)
+
+        if self.params['starify']:
+            self.obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles, p, pg,
+                                                                                             self.params['hull_epsilon'],
+                                                                                             max_compute_time=  self.params['max_compute_time'],
+                                                                                             workspace=workspace,
+                                                                                             previous_clusters=self.obstacle_clusters,
+                                                                                             # dx_prev=self.dp_prev,
+                                                                                             make_convex=self.params['make_convex'],
+                                                                                             verbose=self.verbose)
+            self.timing['obstacle'] = sum(obstacle_timing)
+            self.obstacles_star = [cl.cluster_obstacle for cl in self.obstacle_clusters]
+
+            # import matplotlib.pyplot as plt
+            # tmp_h = []
+            # ax = plt.gca()
+            # for o in self.obstacle_clusters:
+            #     for k in o.kernel_points:
+            #         tmp_h += ax.plot(*k, 'y*')
+            # while not plt.waitforbuttonpress(): pass
+            # [h.remove() for h in tmp_h]
+
+
+            # print(obstacle_timing)
+
+            # self.obstacles_star = []
+            # for cl in self.obstacle_clusters:
+            #     cl_obstacles = cl.obstacles
+            #     for i in range(len(cl_obstacles)):
+            #         cl_obstacles[i].set_xr(cl.cluster_obstacle.xr())
+            #     self.obstacles_star += cl_obstacles
+            # print(obstacle_timing, n_iter)
+            # print("(Cl/Adm/Hull) calculation ({:.2f}/{:.2f}/{:.2f}) [{:.0f}]".format(*obstacle_timing[:-1], n_iter))
+
+        else:
+            self.timing['obstacle'] += 0
+            self.obstacles_star = obstacles
+
+        # if any([o.interior_point(p) for o in self.obstacles_star]):
+        #     import matplotlib.pyplot as plt
+        #     from obstacles.tools import draw_shapely_polygon, is_cw, is_ccw, is_collinear, RayCone
+        #     from starshaped_hull import admissible_kernel
+        #     print("p in obstacles")
+        #     for cl in self.obstacle_clusters:
+        #         _, ax = plt.subplots()
+        #         # cl.cluster_obstacle.draw(ax=ax, fc='r', alpha=0.5)
+        #         for o in cl.obstacles:
+        #             o.draw(ax=ax, show_name=1)
+        #             # if o.id() == 1:
+        #             #     ad_ker = admissible_kernel(o, p)
+        #             #     # tp1, tp2 = o.tangent_points(p)
+        #             #     # if is_ccw(p, tp1, tp2):
+        #             #     #     print('ccw')
+        #             #     # elif is_cw(p, tp1, tp2):
+        #             #     #     print('cw')
+        #             #     # elif is_collinear(p, tp1, tp2):
+        #             #     #     print('collinear')
+        #             #     # ax.plot(*tp1, 'rx')
+        #             #     # ax.plot(*tp2, 'rs')
+        #             #     draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.3)
+        #
+        #         ax.plot(*p, 'o')
+        #         ax.set_xlim([0, 13])
+        #         ax.set_ylim([-1, 11])
+        #         adm_ker_robot = RayCone.intersection_same_apex([admissible_kernel(o, p) for o in cl.obstacles], debug_info={'o': obstacles, 'x': p, 'xlim': [0, 13], 'ylim': [-1, 11]})
+        #         if cl.admissible_kernel is not None:
+        #             # draw_shapely_polygon(cl.admissible_kernel, ax=ax, fc='y', alpha=0.3)
+        #             draw_shapely_polygon(adm_ker_robot.polygon(), ax=ax, fc='y', alpha=0.3)
+        #     plt.show()
+
+        t0 = tic()
+        dist_to_goal = np.linalg.norm(p - pg)
+        if self.params['lin_vel'] > 0:
+            lin_vel = min(self.params['lin_vel'], dist_to_goal / self.params['dt'])
+            unit_mag = True
+        else:
+            lin_vel = 1
+            unit_mag = False
+        dp = lin_vel * f(p, pg, self.obstacles_star, workspace=workspace, unit_magnitude=unit_mag, crep=self.params['crep'],
+                         reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'],
+                         adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'],
+                         convergence_tolerance=self.params['convergence_tolerance'])
+
+        p_next = p + dp * self.params['dt']
+
+        while not all([o.exterior_point(p_next) for o in self.obstacles_star]) and (workspace is None or workspace.interior_point(p_next)):
+            dp *= self.params['dp_decay_rate']
+            p_next = p + dp * self.params['dt']
+            # Additional compute time check
+            if toc(t0) > self.params['max_compute_time']:
+                if self.verbose or True:
+                    print("[Max compute time in soads when adjusting for collision. ")
+                dp *= 0
+                break
+        self.timing['control'] = toc(t0)
+
+        self.dp_prev = dp
+
+        return dp
+
+def f_nominal(r, f_nominal, obstacles, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False,
+      convergence_tolerance=1e-4, d=False):
+    No = len(obstacles)
+
+    if No == 0 and workspace is None:
+        return f_nominal
+
+    ef = [-f_nominal[1], f_nominal[0]]
+    Rf = np.vstack((f_nominal, ef)).T
+
+    r_sh = shapely.geometry.Point(r)
+
+    mu = [obs.reference_direction(r) for obs in obstacles]
+    normal = [obs.normal(r) for obs in obstacles]
+    gamma = [obs.distance_function(r) for obs in obstacles]
+    dist = [obs.polygon().distance(r_sh) for obs in obstacles]
+
+    workspace_avoidance = False
+    if workspace is not None:
+        workspace_avoidance = np.linalg.norm(
+            workspace.xr() - r) > 1e-4  # workspace.polygon().exterior.distance(r_sh) < 1
+
+    if workspace_avoidance:
+        mu += [workspace.reference_direction(r)]
+        normal += [workspace.normal(r)]
+        gamma += [1 / workspace.distance_function(r)]
+        dist += [workspace.polygon().exterior.distance(r_sh)]
+        No += 1
+
+    # Compute weights
+    # w = compute_weights(gamma)
+    w = compute_weights(dist, gamma_lowerlimit=0)
+
+    # Compute obstacle velocities
+    xd_o = np.zeros((2, No))
+    if adapt_obstacle_velocity:
+        for i, obs in enumerate(obstacles):
+            xd_o[:, i] = obs.vel_intertial_frame(r)
+
+    kappa = 0.
+    f_mag = 0.
+    for i in range(No):
+        # Compute basis matrix
+        E = np.zeros((2, 2))
+        E[:, 0] = mu[i]
+        E[:, 1] = [-normal[i][1], normal[i][0]]
+        # Compute eigenvalues
+        D = np.zeros((2, 2))
+        if workspace_avoidance and i == No - 1:
+            inactive_repulsion = normal[i].dot(f_nominal) < 0
+            # inactive_repulsion = False
+            D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity))
+            D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity)
+        else:
+            inactive_repulsion = mu[i].dot(f_nominal) > 0 and normal[i].dot(f_nominal) > 0
+            # inactive_repulsion = False
+            D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity))
+            D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity)
+        # Compute modulation
+        M = E.dot(D).dot(np.linalg.inv(E))
+        # f_i = M.dot(f_nominal)
+        f_i = M.dot(f_nominal - xd_o[:, i]) + xd_o[:, i]
+        # Compute contribution to velocity magnitude
+        f_i_abs = np.linalg.norm(f_i)
+        f_mag += w[i] * f_i_abs
+        # Compute contribution to velocity direction
+        nu_i = f_i / f_i_abs
+        nu_i_hat = Rf.T.dot(nu_i)
+        kappa_i = np.arccos(np.clip(nu_i_hat[0], -1, 1)) * np.sign(nu_i_hat[1])
+        kappa += w[i] * kappa_i
+    kappa_norm = abs(kappa)
+    f_o = Rf.dot([np.cos(kappa_norm), np.sin(kappa_norm) / kappa_norm * kappa]) if kappa_norm > 0. else f_nominal
+
+    if unit_magnitude:
+        f_mag = 1.
+
+    # f_mag = np.linalg.norm(f_nominal)
+
+    return f_mag * f_o
+
+# TODO: Rename to something with linear attractor
+def f(r, rg, obstacles, workspace=None, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False,
+      convergence_tolerance=1e-4, d=False):
+    goal_vector = rg - r
+    goal_dist = np.linalg.norm(goal_vector)
+    if goal_dist < convergence_tolerance:
+        return 0 * r
+
+    No = len(obstacles)
+    fa = goal_vector / goal_dist  # Attractor dynamics
+
+
+    if No == 0 and workspace is None:
+        return fa
+
+    return f_nominal(r, fa, obstacles, workspace, adapt_obstacle_velocity, unit_magnitude, crep, reactivity, tail_effect,
+      convergence_tolerance, d)
+
+    ef = [-fa[1], fa[0]]
+    Rf = np.vstack((fa, ef)).T
+
+    for o in obstacles:
+        if o.boundary_mapping(r) is None:
+            import matplotlib.pyplot as plt
+            o.draw(ax=plt.gca(), fc='r', alpha=0.8)
+            hs = plt.plot(*zip(o.xr(),r), 'b-')
+            hs += plt.plot(*zip(o.xr(), o.xr()+1.1*o.enclosing_ball_diameter*o.reference_direction(r)), 'r--')
+            while not plt.waitforbuttonpress(): pass
+            [h.remove() for h in hs]
+
+    r_sh = shapely.geometry.Point(r)
+
+    mu = [obs.reference_direction(r) for obs in obstacles]
+    normal = [obs.normal(r) for obs in obstacles]
+    gamma = [obs.distance_function(r) for obs in obstacles]
+    dist = [obs.polygon().distance(r_sh) for obs in obstacles]
+
+    workspace_avoidance = False
+    if workspace is not None:
+        workspace_avoidance = np.linalg.norm(workspace.xr()-r) > 1e-4# workspace.polygon().exterior.distance(r_sh) < 1
+
+    if workspace_avoidance:
+        mu += [workspace.reference_direction(r)]
+        normal += [workspace.normal(r)]
+        gamma += [1 / workspace.distance_function(r)]
+        dist += [workspace.polygon().exterior.distance(r_sh)]
+        No += 1
+
+    # Compute weights
+    # w = compute_weights(gamma)
+    w = compute_weights(dist, gamma_lowerlimit=0)
+
+    # Compute obstacle velocities
+    xd_o = np.zeros((2, No))
+    if adapt_obstacle_velocity:
+        for i, obs in enumerate(obstacles):
+            xd_o[:, i] = obs.vel_intertial_frame(r)
+
+    kappa = 0.
+    f_mag = 0.
+    for i in range(No):
+        # Compute basis matrix
+        E = np.zeros((2, 2))
+        E[:, 0] = mu[i]
+        E[:, 1] = [-normal[i][1], normal[i][0]]
+        # Compute eigenvalues
+        D = np.zeros((2, 2))
+        if workspace_avoidance and i == No-1:
+            inactive_repulsion = normal[i].dot(fa) < 0
+            D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity))
+            D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity)
+        else:
+            inactive_repulsion = mu[i].dot(fa) > 0 and normal[i].dot(fa) > 0
+            D[0, 0] = 1 if inactive_repulsion else 1 - crep / (gamma[i] ** (1 / reactivity))
+            D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity)
+        # Compute modulation
+        M = E.dot(D).dot(np.linalg.inv(E))
+        # f_i = M.dot(fa)
+        f_i = M.dot(fa - xd_o[:, i]) + xd_o[:, i]
+        # Compute contribution to velocity magnitude
+        f_i_abs = np.linalg.norm(f_i)
+        f_mag += w[i] * f_i_abs
+        # Compute contribution to velocity direction
+        nu_i = f_i / f_i_abs
+        nu_i_hat = Rf.T.dot(nu_i)
+        kappa_i = np.arccos(np.clip(nu_i_hat[0], -1, 1)) * np.sign(nu_i_hat[1])
+        kappa += w[i] * kappa_i
+    kappa_norm = abs(kappa)
+    f_o = Rf.dot([np.cos(kappa_norm), np.sin(kappa_norm) / kappa_norm * kappa]) if kappa_norm > 0. else fa
+
+    if unit_magnitude:
+        f_mag = 1.
+    return f_mag * f_o
+
+
+def compute_weights(
+    gamma,
+    gamma_lowerlimit=1,
+    gamma_upperlimit=500,
+    weightPow=2
+):
+    """Compute weights based on a distance measure (with no upper limit)"""
+    gamma = np.array(gamma)
+    n_points = gamma.shape[0]
+
+    collision_points = gamma <= gamma_lowerlimit
+
+    if np.any(collision_points):  # at least one
+        if np.sum(collision_points) == 1:
+            return collision_points * 1.0
+        else:
+            # TODO: continuous weighting function
+            w = collision_points * 1.0 / np.sum(collision_points)
+            return w
+
+    gamma = gamma - gamma_lowerlimit
+
+    w = (1 / gamma) ** weightPow
+    if np.sum(w) == 0:
+        return w
+    w = w / np.sum(w)  # Normalization
+    return w
+
+
+def rollout(x, xg, obstacles, step_size=0.01, max_nr_steps=10000, max_distance=np.inf, convergence_threshold=0.05, crep=1., reactivity=1.):
+    xs = np.zeros((2, max_nr_steps))
+    xs[:, 0] = x
+    dist_travelled = 0
+    for k in range(1, max_nr_steps):
+        dist_to_goal = np.linalg.norm(xs[:, k-1]-xg)
+        if dist_to_goal < convergence_threshold or dist_travelled > max_distance:
+            return xs[:, :k]
+        else:
+            step_mag = min(step_size, dist_to_goal)
+            step_dir = f(xs[:, k-1], xg, obstacles, unit_magnitude=1, crep=crep, reactivity=reactivity)
+            xs[:, k] = xs[:, k-1] + step_mag * step_dir
+            while any([o.interior_point(xs[:, k]) for o in obstacles]):
+                step_mag /= 2.
+                xs[:, k] = xs[:, k-1] + step_mag * step_dir
+            dist_travelled += step_mag
+    return xs
+
+
+def draw_streamlines(xg, obstacles, ax, init_pos_list, step_size=0.01, max_nr_steps=10000, max_distance=np.inf,
+                     convergence_threshold=0.05, crep=1., reactivity=1.,
+                     arrow_step=0.5, color='k', linewidth=1, **kwargs):
+    for x0 in init_pos_list:
+        xs = rollout(x0, xg, obstacles, step_size, max_nr_steps, max_distance, convergence_threshold, crep, reactivity)
+        travelled_dist = np.cumsum(np.linalg.norm(np.diff(xs, axis=1), axis=0))
+        travelled_dist = np.insert(travelled_dist, 0, 0)
+        arrow_dist = np.arange(0, travelled_dist[-1], arrow_step)
+        arrows_X = np.interp(arrow_dist, travelled_dist, xs[0, :])
+        arrows_Y = np.interp(arrow_dist, travelled_dist, xs[1, :])
+        arrows_U = np.zeros_like(arrows_X)
+        arrows_V = np.zeros_like(arrows_X)
+        # ax.plot(arrows_X, arrows_Y, 'k*')
+        for j in range(len(arrow_dist)):
+            arrows_U[j], arrows_V[j] = f(np.array([arrows_X[j], arrows_Y[j]]), xg, obstacles, unit_magnitude=1,
+                                         crep=crep, reactivity=reactivity)
+        ax.quiver(arrows_X, arrows_Y, arrows_U, arrows_V, color=color, scale=50, width=0.005, scale_units='width', headlength=2, headaxislength=2)
+        # ax.plot(xs[0, :], xs[1, :], color=color, linewidth=linewidth, **kwargs)
+
+def draw_vector_field(xg, obstacles, ax, workspace=None, xlim=None, ylim=None, n=50, crep=1., reactivity=1., **kwargs):
+    if xlim is None:
+        xlim = ax.get_xlim()
+    if ylim is None:
+        ylim = ax.get_ylim()
+
+    def in_boundary(x):
+        return True if workspace is None else workspace.interior_point(x)
+
+    Y, X = np.mgrid[ylim[0]:ylim[1]:n*1j, xlim[0]:xlim[1]:n*1j]
+    U = np.zeros(X.shape)
+    V = np.zeros(X.shape)
+    mask = np.zeros(U.shape, dtype=bool)
+    for i in range(X.shape[1]):
+        for j in range(X.shape[0]):
+            x = np.array([X[i, j], Y[i, j]])
+            if in_boundary(x) and np.all([o.exterior_point(x) for o in obstacles]):
+                U[i, j], V[i, j] = f(x, xg, obstacles, workspace=workspace, unit_magnitude=1, crep=crep, reactivity=reactivity)
+            else:
+                mask[i, j] = True
+                U[i, j] = np.nan
+    U = np.ma.array(U, mask=mask)
+    # return ax.quiver(X, Y, U, V)
+    return ax.streamplot(X, Y, U, V, **kwargs)
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/__init__.py b/python/ur_simple_control/path_generation/star_navigation/robot/__init__.py
new file mode 100644
index 0000000..a0f0b8a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/robot/__init__.py
@@ -0,0 +1,7 @@
+"""
+The :mod:`robot` module implements mobile robots for obstacle avoidance.
+"""
+from .mobile_robot import MobileRobot
+from .unicycle import Unicycle
+from .bicycle import Bicycle
+from .omnidirectional import Omnidirectional
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py b/python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py
new file mode 100644
index 0000000..a2f4ec6
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/robot/bicycle.py
@@ -0,0 +1,55 @@
+import numpy as np
+from robot import MobileRobot
+
+
+class Bicycle(MobileRobot):
+
+    def __init__(self, width, vel_min=None, vel_max=None, steer_max=None, name='robot'):
+        self.vmax = vel_max[0]
+        x_min = [-np.inf] * 3 + [-steer_max]
+        x_max = [np.inf] * 3 + [steer_max]
+        super().__init__(nu=2, nx=4, width=width, name=name, u_min=vel_min, u_max=vel_max, x_min=x_min, x_max=x_max)
+
+    # [x,y of backwheel, orientation, steering angle]
+    # def f(self, x, u):
+    #     return [u[0] * np.cos(x[2]),
+    #             u[0] * np.sin(x[2]),
+    #             u[0] * np.tan(u[1]) / self.width]
+    def f(self, x, u):
+        return [u[0] * np.cos(x[2]),
+                u[0] * np.sin(x[2]),
+                u[0] * np.tan(u[1]) / self.width,
+                u[1]]
+    # def f(self, x, u):
+    #     return [u[0] * np.cos(x[2]) * np.cos(x[3]),
+    #             u[0] * np.sin(x[2]) * np.cos(x[3]),
+    #             u[0] * np.sin(x[3]) / self.width,
+    #             u[1]]
+
+    def h(self, x):
+        return [x[0] + self.width/2 * np.cos(x[2]),
+                x[1] + self.width/2 * np.sin(x[2])]
+
+    def vel_min(self):
+        return self.u_min
+
+    def vel_max(self):
+        return self.u_max
+
+    def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10, **kwargs):
+        h = super(Bicycle, self).init_plot(ax=ax, color=color, alpha=alpha)
+        h += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color)
+        h += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w')
+        h += ax.plot(0, 0, color=color, alpha=alpha)
+        return h
+
+    def update_plot(self, x, handles):
+        super(Bicycle, self).update_plot(x, handles)
+        handles[1].set_data(self.h(x))
+        handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[1].set_markersize(handles[1].get_markersize())
+        handles[2].set_data(x[0], x[1])
+        handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[2].set_markersize(handles[2].get_markersize())
+        handles[3].set_data([x[0], x[0] + self.width * np.cos(x[2])],
+                            [x[1], x[1] + self.width * np.sin(x[2])])
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py b/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py
new file mode 100644
index 0000000..0177489
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/robot/mobile_robot.py
@@ -0,0 +1,74 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from abc import ABC, abstractmethod
+import numpy as np
+
+
+class MobileRobot(ABC):
+    def __init__(self, nu, nx, radius, name, u_min=None, u_max=None, x_min=None, x_max=None,
+                 integration_method='euler'):
+        self.nx = nx
+        self.nu = nu
+        self.radius = radius
+        self.name = name
+        def valid_u_bound(bound): return bound is not None and len(bound) == self.nu
+        def valid_q_bound(bound): return bound is not None and len(bound) == self.nx
+        self.u_min = u_min if valid_u_bound(u_min) else [-np.inf] * self.nu
+        self.u_max = u_max if valid_u_bound(u_max) else [np.inf] * self.nu
+        self.x_min = x_min if valid_q_bound(x_min) else [-np.inf] * self.nx
+        self.x_max = x_max if valid_q_bound(x_max) else [np.inf] * self.nx
+        self.integration_method = integration_method
+
+    @abstractmethod
+    def f(self, x, u):
+        """ Forward dynamics ? """
+        pass
+
+    @abstractmethod
+    def h(self, q):
+        """ Forward kinematics """
+        pass
+
+    @abstractmethod
+    def vel_min(self):
+        pass
+
+    @abstractmethod
+    def vel_max(self):
+        pass
+
+    def move(self, x, u, dt):
+        u_sat = np.clip(u, self.u_min, self.u_max)
+        if self.integration_method == 'euler':
+            x_next = x + np.array(self.f(x, u_sat)) * dt
+        elif self.integration_method == 'RK4':
+            k1 = np.array(self.f(x, u_sat))
+            k2 = np.array(self.f(x + dt / 2 * k1, u_sat))
+            k3 = np.array(self.f(x + dt / 2 * k2, u_sat))
+            k4 = np.array(self.f(x + dt * k3, u_sat))
+            x_next = x + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
+        else:
+            print("Not a valid integration method")
+        x_next = np.clip(x_next, self.x_min, self.x_max)
+        return x_next, u_sat
+
+#    def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10, **kwargs):
+#        if ax is None:
+#            _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+#        if self.radius == 0:
+#            handles = plt.plot(0, 0, '+', color=color, alpha=alpha, markersize=markersize, label=self.name, **kwargs)
+#        else:
+#            handles = [patches.Circle((0, 0), self.radius, color=color, alpha=alpha, label=self.name, **kwargs)]
+#            ax.add_patch(handles[0])
+#        return handles, ax
+#
+#    def update_plot(self, x, handles):
+#        if self.radius == 0:
+#            handles[0].set_data(*self.h(x))
+#        else:
+#            handles[0].set(center=self.h(x))
+#
+#    def draw(self, x, **kwargs):
+#        handles, ax = self.init_plot(**kwargs)
+#        self.update_plot(x, handles)
+#        return handles, ax
diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py b/python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py
new file mode 100644
index 0000000..f476fac
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/robot/omnidirectional.py
@@ -0,0 +1,23 @@
+import numpy as np
+from robot import MobileRobot
+
+
+class Omnidirectional(MobileRobot):
+
+    def __init__(self, radius, vel_max, name='robot'):
+        vel_max = vel_max * np.ones(2)
+        self.vmax = float(np.linalg.norm(vel_max))
+        super().__init__(nu=2, nx=2, radius=radius, name=name, u_min=(-vel_max).tolist(), u_max=(vel_max).tolist())
+
+    def f(self, x, u):
+        return [u[0],
+                u[1]]
+
+    def h(self, x):
+        return x[:2]
+
+    def vel_min(self):
+        return self.u_min
+
+    def vel_max(self):
+        return self.u_max
diff --git a/python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py b/python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py
new file mode 100644
index 0000000..5ae1226
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/robot/unicycle.py
@@ -0,0 +1,68 @@
+import numpy as np
+from robot import MobileRobot
+import matplotlib.pyplot as plt
+
+
+def get_triangle_vertices(center, orientation, side_length):
+    # calculate the distance from center to vertex
+    dist = side_length / (2 * np.sin(np.radians(60)))
+
+    # calculate the x and y offsets from center to vertex
+    x_offset = dist * np.cos(orientation)
+    y_offset = dist * np.sin(orientation)
+
+    # calculate the three vertices
+    vertex1 = (center[0] + x_offset, center[1] + y_offset)
+    vertex2 = (center[0] + x_offset * np.cos(np.radians(120)) - y_offset * np.sin(np.radians(120)),
+               center[1] + x_offset * np.sin(np.radians(120)) + y_offset * np.cos(np.radians(120)))
+    vertex3 = (center[0] + x_offset * np.cos(np.radians(240)) - y_offset * np.sin(np.radians(240)),
+               center[1] + x_offset * np.sin(np.radians(240)) + y_offset * np.cos(np.radians(240)))
+
+    return (vertex1, vertex2, vertex3)
+
+class Unicycle(MobileRobot):
+
+    def __init__(self, width, vel_min=None, vel_max=None, name='robot'):
+        self.vmax = vel_max[0]
+        super().__init__(nu=2, nx=3, width=width, name=name, u_min=vel_min, u_max=vel_max)
+
+    def f(self, x, u):
+        return [u[0] * np.cos(x[2]),  # vx
+                u[0] * np.sin(x[2]),  # vy
+                u[1]]                 # wz
+
+
+    def move(self, x, u, dt):
+        u_sat = np.clip(u, self.u_min, self.u_max)
+        x_next = x + np.array(self.f(x, u_sat)) * dt
+        x_next = np.clip(x_next, self.x_min, self.x_max)
+        while x_next[2] > 2*np.pi:
+            x_next[2] -= 2 * np.pi
+        while x_next[2] <= -0*np.pi:
+            x_next[2] += 2 * np.pi
+        return x_next, u_sat
+
+
+    def h(self, x):
+        return x[:2] # x, y
+
+    def vel_min(self):
+        return self.u_min
+
+    def vel_max(self):
+        return self.u_max
+
+    def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10):
+        handles, ax = super(Unicycle, self).init_plot(ax=ax, color=color, alpha=alpha)
+        handles += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color)
+        handles += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w')
+        return handles, ax
+
+    def update_plot(self, x, handles):
+        super(Unicycle, self).update_plot(x, handles)
+        handles[1].set_data([x[0]], [x[1]])
+        handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[1].set_markersize(handles[1].get_markersize())
+        handles[2].set_data([x[0]], [x[1]])
+        handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[2].set_markersize(handles[2].get_markersize())
diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py b/python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py
new file mode 100644
index 0000000..993f33d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/scripts/compute_time.py
@@ -0,0 +1,135 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from config.load_config import load_config
+from visualization import PFMPCGUI
+
+make_video = 0
+
+ctrl_param_file = 'pfmpc_ds_params.yaml'
+robot_type_id = 3
+scene_id = 13
+verbosity = 0
+
+scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity)
+
+workspaces = scene.workspace if isinstance(scene.workspace, list) else [scene.workspace]
+
+
+# Settings
+T_max = 200
+theta_max = 100
+dt = 0.01
+ctrl_sim_dt_ratio = int(controller.params['dt'] / dt)
+show_gui = True
+
+
+if show_gui:
+    gui = PFMPCGUI(robot, scene, x0, scene.xlim, scene.ylim,
+                       controller=controller, robot_alpha=1., robot_color='k',
+                       obstacles_star_alpha=0.2, obstacles_star_show_reference=0,
+                       obstacles_star_color='b',
+                       # workspace_rho_color='None',
+                       # show_obs_name=1,
+                       reference_color='y',
+                       reference_markersize=10,
+                       pg_markersize=10, pg_color='b', pg_marker='*',
+                       theta_pos_color='g', theta_pos_marker='*', theta_pos_markersize=10,
+                       s1_pos_color='g', s1_pos_marker='+', s1_pos_markersize=0,
+                       mpc_path_linestyle='-', mpc_path_linewidth=4, mpc_path_color='None',
+                       mpc_tunnel_color='r',
+                       travelled_path_linestyle='--', travelled_path_linewidth=3,
+                       receding_path_color='g',
+                       receding_path_linewidth=2,
+                       show_time=1, show_timing=1, show_axis=0
+                       )
+
+
+fig, ax = plt.subplots()
+linestyles = ['-', '--', ':', '-.']
+rows = []
+data = []
+
+x0_0s = [-4, -2, 0, 2]
+for i, x0_0 in enumerate(x0_0s):
+    x0[:2] = [x0_0, -5.]
+
+
+
+    # Initialize
+    k = 0
+    x = x0
+    active_ws_idx = 0
+    converged = False
+    collision = False
+    timing = {'workspace': [], 'target': [], 'mpc': []}
+    controller.reset()
+
+
+    while k*dt <= T_max and controller.theta <= theta_max and not converged and not collision:
+        p = robot.h(x)
+        if active_ws_idx + 1 < len(workspaces):
+            ws_smaller = workspaces[active_ws_idx + 1].dilated_obstacle(-0.1, id='temp')
+            if controller.rhrp_path is not None and all([ws_smaller.interior_point(r) for r in controller.rhrp_path]):
+                active_ws_idx += 1
+                if len(scene.reference_path) == 1:
+                    controller.set_reference_path([scene.ws_attractors[active_ws_idx]])
+
+        # Move obstacles
+        scene.step(dt, p)
+
+        # Control update
+        if k % ctrl_sim_dt_ratio == 0:
+            if k*dt % 2 == 0:
+                print(k*dt)
+            # Compute mpc
+            controller.update_policy(x, scene.obstacles, workspace=workspaces[active_ws_idx])
+            # Update timing
+            close_to_convergence = (np.linalg.norm(controller.r_plus - controller.pg) < controller.rho) and (controller.theta >= controller.theta_g)
+            if not close_to_convergence:
+                for key in timing.keys():
+                    timing[key] += [controller.timing[key]]
+
+            if show_gui:
+                gui.update(x, k*dt, controller)
+                plt.pause(0.005)
+
+        # Robot integration
+        u = controller.compute_u(x)
+        x, _ = robot.move(x, u, dt)
+        k += 1
+
+        # Convergence and collision check
+        converged = controller.theta >= controller.theta_g and np.linalg.norm(robot.h(x)-scene.reference_path[-1]) < controller.params['convergence_tolerance']
+        collision = any([o.interior_point(robot.h(x)) for o in scene.obstacles])
+        if collision:
+            print("Collision")
+
+
+    ax.plot(timing['workspace'],'b', ls=linestyles[i])
+    ax.plot(timing['target'],'r', ls=linestyles[i])
+    ax.plot(timing['mpc'],'g', ls=linestyles[i])
+    ax.legend(['Workspace modification', 'Target path generation', 'MPC'])
+
+    for k, v in timing.items():
+        rows += [k]
+        data += [[np.min(v), np.max(v), np.mean(v)]]
+    rows += [""]
+
+# Plot bars and create text labels for the table
+columns = ['min', 'max', 'mean']
+cell_text = []
+for row in range(len(rows)):
+    if row % 2 == 0:
+        cell_text.append(["", "", ""])
+    cell_text.append(["{:.2f}".format(t) for t in data[row]])
+# Add a table at the bottom of the axes
+the_table = plt.table(cellText=cell_text,
+                      rowLabels=rows,
+                      colLabels=columns,
+                      loc='bottom')
+
+fig.tight_layout()
+plt.xticks([])
+plt.title('Computation time')
+plt.show()
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py b/python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py
new file mode 100755
index 0000000..85a71d3
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/scripts/run_simulation.py
@@ -0,0 +1,304 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from config.load_config import load_config
+from visualization import PFMPCGUI, VideoWriter, StarobsGUI
+from starworlds.utils.misc import tic, toc
+from motion_control.pfmpc_ds.motion_controller import ControlMode
+from motion_control.soads import draw_vector_field
+
+make_video = 0
+
+# ctrl_param_file = 'tunnel_mpc_params.yaml'
+ctrl_param_file = 'pfmpc_ds_params.yaml'
+# ctrl_param_file = 'pfmpc_obstacle_constraints_params.yaml'
+# ctrl_param_file = 'pfmpc_artificial_reference_params.yaml'
+# ctrl_param_file = 'dmp_ctrl_params.yaml'
+robot_type_id = 2
+scene_id = None
+scene_id = 17
+verbosity = 0
+show_mpc_solution = 0
+show_mpc_cost = 0
+show_u_history = 1
+show_timing = 1
+
+scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity)
+
+# x0[:2] = [0., -5.]
+# x0s_10 = [[-2.5, 1.5, 0.], [2.4, -3.5, 0.], [3, .5, 0.], [2, 2.5, 0.], [-5, -5.5, 0.], [-2.8, -3.8, 0.]]
+# x0 = np.array(x0s_10[4])
+
+if hasattr(controller, 'mpc'):
+    gui = PFMPCGUI(robot, scene, x0, scene.xlim, scene.ylim,
+                   show_mpc_solution=show_mpc_solution, show_mpc_cost=show_mpc_cost, show_u_history=show_u_history,
+                   controller=controller, robot_alpha=1., robot_color='k',
+                   obstacles_star_alpha=0.2, obstacles_star_show_reference=0,
+                   obstacles_star_color='b',
+                   workspace_rho_color='b',
+                   # show_obs_name=1,
+                   reference_color='y',
+                   reference_markersize=10,
+                   pg_markersize=10, pg_color='b', pg_marker='*',
+                   theta_pos_color='g', theta_pos_marker='*', theta_pos_markersize=10,
+                   s1_pos_color='g', s1_pos_marker='+', s1_pos_markersize=0,
+                   mpc_path_linestyle=':', mpc_path_linewidth=4, mpc_path_color='None',
+                   mpc_tunnel_color='None',
+                   travelled_path_linestyle='-', travelled_path_linewidth=3,
+                   receding_path_color='g',
+                   receding_path_linewidth=2,
+                   indicate_sbc=1,
+                   show_time=1, show_timing=show_timing, show_axis=1,
+                   )
+else:
+    gui = StarobsGUI(robot, scene, x0, scene.xlim, scene.ylim)
+
+workspaces = scene.workspace if isinstance(scene.workspace, list) else [scene.workspace]
+
+# Initialize
+T_max = 50
+theta_max = 100
+dt = 0.01
+dt_gui = 0.1
+ctrl_sim_dt_ratio = int(controller.params['dt'] / dt)
+gui_sim_dt_ratio = int(dt_gui / dt)
+k = 0
+x = x0
+active_ws_idx = 0
+converged = False
+collision = False
+timing = {'workspace': [], 'target': [], 'mpc': []}
+if 'workspace_detailed' in controller.timing:
+    workspace_timing_detailed = {'cluster': [], 'admker': [], 'hull': [], 'convex': []}
+
+# Init video writing
+if make_video:
+    frame_cntr = 0
+    video_name = input("Video file name: ")
+    video_writer = VideoWriter(video_name, 1/dt_gui)
+    gui.paused = False
+else:
+    # Init gui plot
+    gui.update(x, k*dt)
+    plt.pause(0.005)
+
+
+ls = []
+
+# for i, attr in enumerate(scene.ws_attractors[:-1]):
+#     gui.ax.plot(*attr, 'y*', markersize=10)
+# gui.ax.plot(*scene.ws_attractors[-1], 'g*', markersize=14, zorder=-1)
+
+# xlim_closeup, ylim_closeup = [-4.2, -0.8], [-5.2, -2.4]
+# gui.ax.plot([xlim_closeup[0], xlim_closeup[1], xlim_closeup[1], xlim_closeup[0], xlim_closeup[0]],
+#             [ylim_closeup[0], ylim_closeup[0], ylim_closeup[1], ylim_closeup[1], ylim_closeup[0]], 'k-')
+
+while gui.fig_open and k*dt <= T_max and controller.theta <= theta_max and not converged and not collision:
+
+    if gui.paused and not gui.step_once:
+        gui.fig.waitforbuttonpress()
+
+    p = robot.h(x)
+    if active_ws_idx + 1 < len(workspaces):
+        ws_smaller = workspaces[active_ws_idx + 1].dilated_obstacle(-0.1, id='temp')
+        if controller.rhrp_path is not None and all([ws_smaller.interior_point(r) for r in controller.rhrp_path]):
+            active_ws_idx += 1
+            if len(scene.reference_path) == 1:
+                controller.set_reference_path([scene.ws_attractors[active_ws_idx]])
+                scene.reference_path = [scene.ws_attractors[active_ws_idx]]
+
+    # Move obstacles
+    scene.step(dt, p)
+
+    # Control update
+    if k % ctrl_sim_dt_ratio == 0:
+        gui.step_once = False
+
+        # # For Fig 9.a
+        # if k == 1120:
+        #     nominal_rhrp, _ =  controller.nominal_rhrp(controller.r_plus, scene.obstacles[0].polygon())
+
+        # Compute mpc
+        controller.update_policy(x, scene.obstacles, workspace=workspaces[active_ws_idx])
+        u = controller.compute_u(x)
+
+        # Add input noise
+        u_noise = [.1 * np.random.randn(), .1 * np.random.randn()]
+        print(u_noise[0]/1, u_noise[1]/1.5)
+        u[0] = u[0] + u_noise[0]
+        u[1] = u[1] + u_noise[1]
+
+        gui.update(x, k*dt, controller, u)
+        # Update timing
+        if k > 0:
+            for key in timing.keys():
+                if key == 'tot':
+                    continue
+                timing[key] += [controller.timing[key]]
+            if 'workspace_detailed' in controller.timing and controller.timing['workspace_detailed'] is not None:
+                for i, key in enumerate(workspace_timing_detailed.keys()):
+                    workspace_timing_detailed[key] += [controller.timing['workspace_detailed'][i]]
+
+        # if controller.mode == ControlMode.SBC:
+        #     print("SBC at time {:.2f} (k={:.0f})".format(k*dt, k))
+
+    # For Fig 8.b
+    # if k == 1250:
+    #     xmin, ymin, xmax, ymax = controller.workspace_rho.polygon().bounds
+    #     draw_vector_field(controller.pg, controller.obstacles_star, xlim=[xmin, xmax], ylim=[ymin, ymax],
+    #                       ax=gui.ax, n=40, density=0.5, linewidth=1, color='tab:olive', zorder=-1)
+    #     plt.pause(0.5)
+    #     while not gui.fig.waitforbuttonpress(): pass
+    #
+    # For Fig 8.c
+    # if k == 1360:
+    #     print("SBC at time {:.2f} (k={:.0f})".format(k*dt, k))
+    #     xlim, ylim = gui.ax.get_xlim(), gui.ax.get_ylim()
+    #     gui.ax.set_xlim([-5.5, -2.6])
+    #     gui.ax.set_ylim([-4, 2])
+    #     draw_vector_field(controller.pg, controller.obstacles_star, workspace=controller.workspace_rho,
+    #                       ax=gui.ax, n=80, density=1.4, linewidth=1, color='tab:olive', zorder=-1)
+    #     robot.draw(x, ax=gui.ax, color='k', markersize=26, alpha=1)
+    #     plt.pause(0.5)
+    #     while not gui.fig.waitforbuttonpress(): pass
+    #     gui.ax.set_xlim(xlim)
+    #     gui.ax.set_ylim(ylim)
+
+    # # For Fig 8.d
+    # if k == 1520:
+    #     xlim, ylim = gui.ax.get_xlim(), gui.ax.get_ylim()
+    #     gui.ax.set_xlim([-5.5, -2.6])
+    #     gui.ax.set_ylim([-4, 2])
+    #     gui.mpc_path_handle.set_color('k')
+    #     gui.mpc_path_handle.set_linestyle('-')
+    #     robot.draw(x, ax=gui.ax, color='k', markersize=26, alpha=1)
+    #     plt.pause(0.5)
+    #     while not gui.fig.waitforbuttonpress(): pass
+    #     gui.ax.set_xlim(xlim)
+    #     gui.ax.set_ylim(ylim)
+    #     gui.mpc_path_handle.set_color('None')
+
+    # # For Fig 9.a
+    # if k == 1120:
+    #     gui.ax.plot(*nominal_rhrp.xy, 'm-', linewidth=3, zorder=1)
+    #     for o in controller.obstacles_rho:
+    #         o.draw(ax=gui.ax, fc='y', zorder=0, show_reference=0)
+    #     while not gui.fig.waitforbuttonpress(): pass
+
+    # GUI update
+    if k % gui_sim_dt_ratio == 0:
+        gui.update(x, k*dt)
+
+        if make_video:
+
+            # if scene_id == 19:
+            #     wait_frames = {1/dt: 2, 5/dt: 2, 6.6/dt: 2}
+            #     for j in range(2/gui_sim_dt_ratio):
+            #         video_writer.add_frame(gui.fig)
+
+            video_writer.add_frame(gui.fig, frame_cntr)
+            frame_cntr += 1
+            print("[VideoWriter] wrote frame at time {:.2f}/{:.2f}".format(k * dt, T_max))
+        else:
+            plt.pause(0.005)
+
+    #
+    # r = controller.rhrp_path[-1]
+    # mu = controller.obstacles_star[0].reference_direction(r)
+    # n = controller.obstacles_star[0].normal(r)
+    # b = controller.obstacles_star[0].boundary_mapping(r)
+    # gui.ax.quiver(*b, *mu)
+    # gui.ax.quiver(*b, *n, color='r')
+    #
+    # from motion_control.soads import compute_weights
+    # import shapely
+    # gammas = [obs.distance_function(p) for obs in controller.obstacles_star]
+    # xrs = [obs.xr() for obs in controller.obstacles_star]
+    # ds = [obs.polygon().distance(shapely.geometry.Point(p)) for obs in controller.obstacles_star]
+    # if controller.workspace_rho is not None:
+    #     xrs += [controller.workspace_rho.xr()]
+    #     gammas += [1 / controller.workspace_rho.distance_function(p+0.01)]
+    #     ds += [controller.workspace_rho.polygon().exterior.distance(shapely.geometry.Point(p))]
+    # ws = compute_weights(gammas)
+    # ws = compute_weights(ds, gamma_lowerlimit=0)
+    # if ls:
+    #     [l.remove() for l in ls if l is not None]
+    # ls = []
+    # for xr, w, gamma, d in zip(xrs, ws, gammas, ds):
+    #     ls += [gui.ax.text(*xr, "{:.2f}".format(w))]
+
+    # while not gui.fig.waitforbuttonpress(): pass
+    # gui.ax.set_xlim(np.array(xlim_closeup) + np.array([-0.01, 0.01]))
+    # gui.ax.set_ylim(np.array(ylim_closeup) + np.array([-0.01, 0.01]))
+
+
+    # Robot integration
+    u = controller.compute_u(x)
+    x, _ = robot.move(x, u, dt)
+    k += 1
+
+    # Convergence and collision check
+    converged = controller.theta >= controller.theta_g and np.linalg.norm(robot.h(x)-scene.reference_path[-1]) < controller.params['convergence_tolerance']
+    # print(np.linalg.norm(robot.h(x)-scene.reference_path[-1]), controller.params['convergence_tolerance'], converged)
+    collision = any([o.interior_point(robot.h(x)) for o in scene.obstacles])
+    if collision:
+        print("Collision")
+
+if make_video:
+    # close video writer
+    video_writer.release()
+    print("Finished")
+    fig_open = False
+else:
+    gui.update(x, k*dt, controller, u)
+    gui.ax.set_title("Time: {:.1f} s. Finished".format(k*dt))
+
+    if show_timing:
+        t = [np.array(timing['workspace']), np.array(timing['target']), np.array(timing['mpc'])]
+        fig, ax = plt.subplots()
+        ax.fill_between(range(len(timing['workspace'])), 0*t[0], t[0])
+        ax.fill_between(range(len(timing['workspace'])), t[0], t[0]+t[1])
+        ax.fill_between(range(len(timing['workspace'])), t[0]+t[1], t[0]+t[1]+t[2])
+        # ax.plot(np.array(timing['workspace'])+np.array(timing['target']))
+        # ax.plot(np.array(timing['workspace'])+np.array(timing['target'])+np.array(timing['mpc']),'--')
+        # ax.plot(timing['tot'], '--')
+        ax.legend(['Environment modification', 'RHRP', 'MPC'])
+
+        columns = ['min', 'max', 'mean']
+        rows = []
+        data = []
+        for k, v in timing.items():
+            first_zero = np.where(np.array(v) == 0)[0][0]
+            rows += [k]
+            data += [[np.min(v[:first_zero]), np.max(v), np.mean(v)]]
+        # if 'workspace_detailed' in controller.timing:
+        #     rows += [""]
+        #     data += [[None, None, None]]
+        #     for k, v in workspace_timing_detailed.items():
+        #         rows += [k]
+        #         data += [[np.min(v), np.max(v), np.mean(v)]]
+
+        # Plot bars and create text labels for the table
+        cell_text = []
+        for row in range(len(rows)):
+            if data[row][0] is None:
+                cell_text.append(["", "", ""])
+            else:
+                cell_text.append(["{:.2f}".format(t) for t in data[row]])
+        # Add a table at the bottom of the axes
+        the_table = plt.table(cellText=cell_text,
+                              rowLabels=rows,
+                              colLabels=columns,
+                              loc='bottom')
+
+        # Adjust layout to make room for the table:
+        # plt.subplots_adjust(left=0.2, bottom=0.4)
+        fig.tight_layout()
+        plt.subplots_adjust(hspace=0.2)
+
+        # plt.ylabel(f"Loss in ${value_increment}'s")
+        # plt.yticks(values * value_increment, ['%d' % val for val in values])
+        plt.xticks([])
+        plt.title('Computation time')
+
+    # Wait until figure closed when converged
+    plt.show()
diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py
new file mode 100644
index 0000000..f07c299
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_controllers.py
@@ -0,0 +1,158 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from config.load_config import load_config
+from visualization import PFMPCGUI, VideoWriter
+from motion_control.pfmpc_ds import pol2pos
+import shapely
+
+# ------------------------------- #
+
+make_video = 1
+
+# Scenario settings
+ctrl_param_files = ['pfmpc_ds_params.yaml','pfmpc_obstacle_constraints_params.yaml', 'pfmpc_artificial_reference_params.yaml']
+ctrl_param_files = ['pfmpc_ds_params.yaml']
+robot_type_id = 3
+scene_id = 15
+verbosity = 0
+
+# Simulation settings
+T_max = 200
+dt = 0.01
+dt_gui = 0.1
+
+# GUI settings
+ctrl_colors = ['k', 'r', 'y']
+gui_robot_idx = 0  # GUI environment is shown for robot corresponding to this x0 idx
+
+# ------------------------------- #
+
+
+N = len(ctrl_param_files)
+travelled_path_handles = [None] * N
+robot_handles = [[]] * N
+controllers = [[]] * N
+scenes = [[]] * N
+
+assert gui_robot_idx < N
+scenes[gui_robot_idx], robot, controllers[gui_robot_idx], x0 = load_config(ctrl_param_file=ctrl_param_files[gui_robot_idx], robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity)
+gui = PFMPCGUI(robot, scenes[gui_robot_idx], x0, scenes[gui_robot_idx].xlim, scenes[gui_robot_idx].ylim,
+               controller=controllers[gui_robot_idx], robot_color=ctrl_colors[gui_robot_idx],
+               reference_color='y',
+               theta_pos_color='None',
+               # obstacles_star_color='None',
+               # pg_color='None',
+               workspace_rho_color='tab:blue',
+               # indicate_sbc=False,
+               mpc_artificial_path_color='None',
+               s1_pos_color='None',
+               # mpc_path_color='None',
+               # mpc_tunnel_color='None',
+               # receding_path_color='None',
+               travelled_path_color=ctrl_colors[gui_robot_idx]
+               )
+
+for i in range(N):
+    if i == gui_robot_idx:
+        continue
+    scenes[i], _, controllers[i], _ = load_config(ctrl_param_file=ctrl_param_files[i], robot_type_id=3, scene_id=scene_id, verbosity=verbosity)
+    robot_handles[i], _ = robot.draw(x0, ax=gui.ax, markersize=10, color=ctrl_colors[i], alpha=1)
+    travelled_path_handles[i] = gui.ax.plot([], [], c=ctrl_colors[i], ls=travelled_path_linestyle, lw=travelled_path_linewidth, zorder=0)[0]
+
+# Initialize simulation
+ctrl_sim_dt_ratio = int(controllers[0].params['dt'] / dt)
+gui_sim_dt_ratio = int(dt_gui / dt)
+k = 0
+tmp_hs = []
+xs = np.tile(x0, (N, 1))
+travelled_path = xs[:, :2]
+converged = [False] * N
+collision = False
+workspaces = scenes[0].workspace if isinstance(scenes[0].workspace, list) else [scenes[0].workspace]
+active_ws_idcs = [0] * N
+
+# Init video writing
+if make_video:
+    video_name = input("Video file name: ")
+    video_writer = VideoWriter(video_name, 1/dt_gui)
+    gui.paused = False
+else:
+    # Init gui plot
+    gui.update(xs[gui_robot_idx, :], k*dt)
+    plt.pause(0.005)
+
+
+while gui.fig_open and k*dt <= T_max and not all(converged) and not collision:
+    if gui.paused and not gui.step_once:
+        gui.fig.waitforbuttonpress()
+
+    # Move obstacles
+    for i in range(N):
+        scenes[i].step(dt, robot.h(xs[i, :]))
+
+    # Control update
+    if k % ctrl_sim_dt_ratio == 0:
+        gui.step_once = False
+
+        for i in range(N):
+            # Check current workspace
+            if active_ws_idcs[i] + 1 < len(workspaces):
+                ws_smaller = workspaces[active_ws_idcs[i] + 1].dilated_obstacle(-0.1, id='temp')
+                if controllers[i].rhrp_path is not None and all(
+                        [ws_smaller.interior_point(r) for r in controllers[i].rhrp_path]):
+                    active_ws_idcs[i] += 1
+                    if len(scenes[i].reference_path) == 1:
+                        controllers[i].set_reference_path([scenes[i].ws_attractors[active_ws_idcs[i]]])
+            # Update control policy
+            controllers[i].update_policy(xs[i, :], scenes[i].obstacles, workspace=workspaces[active_ws_idcs[i]])
+
+    # GUI update
+    if k % gui_sim_dt_ratio == 0:
+        [h.remove() for h in tmp_hs if not None]
+        tmp_hs = []
+        if k % ctrl_sim_dt_ratio == 0:
+            gui.update(xs[gui_robot_idx, :], k*dt, controllers[gui_robot_idx], controllers[gui_robot_idx].compute_u(xs[gui_robot_idx, :]))
+        else:
+            gui.update(xs[gui_robot_idx, :], k*dt)
+        for i in range(N):
+            if i == gui_robot_idx:
+                continue
+            robot.update_plot(xs[i, :], robot_handles[i])
+            travelled_path_handles[i].set_data(travelled_path[i, ::2], travelled_path[i, 1::2])
+
+        if make_video:
+            video_writer.add_frame(gui.fig)
+            print("[VideoWriter] wrote frame at time {:.2f}/{:.2f}".format(k * dt, T_max))
+        else:
+            plt.pause(0.005)
+
+    # Robot integration
+    for i in range(N):
+        if converged[i]:
+            u = np.zeros(robot.nu)
+        else:
+            u = controllers[i].compute_u(xs[i, :])
+        xs[i, :], _ = robot.move(xs[i, :], u, dt)
+    travelled_path = np.hstack((travelled_path, xs[:, :2]))
+
+    # Convergence and collision check
+    for i in range(N):
+        converged[i] = controllers[i].theta >= controllers[i].theta_g and np.linalg.norm(robot.h(xs[i, :])-scenes[i].reference_path[-1]) < controllers[i].params['convergence_tolerance']
+        if any([o.interior_point(robot.h(xs[i, :])) for o in scenes[i].obstacles]):
+            collision = True
+            print("Collision")
+
+    k += 1
+
+
+if make_video:
+    # close video writer
+    video_writer.release()
+    print("Finished")
+    fig_open = False
+else:
+    gui.update(xs[gui_robot_idx, :], k*dt)
+    gui.ax.set_title("Time: {:.1f} s. Finished".format(k*dt))
+
+    # Wait until figure closed when converged
+    plt.show()
diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py
new file mode 100644
index 0000000..18a98a5
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/scripts/simulate_several_initpos.py
@@ -0,0 +1,190 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from config.load_config import load_config
+from visualization import PFMPCGUI, VideoWriter
+from motion_control.pfmpc_ds import pol2pos
+import shapely
+
+# ------------------------------- #
+
+make_video = 0
+
+# Scenario settings
+ctrl_param_file = 'pfmpc_ds_params.yaml'
+robot_type_id = 1
+scene_id = 22
+verbosity = 0
+
+# Simulation settings
+T_max = 200
+dt = 0.01
+dt_gui = 0.1
+
+# GUI settings
+gui_robot_idx = 3  # GUI environment is shown for robot corresponding to this x0 idx
+show_mpc_tunnel = 0  # 0: Don't show.  1: Show for gui robot.  2: Show for all robots.
+show_mpc_sol = 0  # 0: Don't show.  1: Show for gui robot.  2: Show for all robots.
+show_travelled_path = 2  # 0: Don't show.  1: Show for gui robot.  2: Show for all robots.
+
+gui_robot_color, robot_color = 'tab:orange', 'k'
+gui_robot_color, robot_color = 'k', 'k'
+travelled_path_color, travelled_path_linestyle, travelled_path_linewidth = 'k', '--', 2
+mpc_sol_color, mpc_sol_linestyle, mpc_sol_linewidth = 'tab:orange', '-', 1
+mpc_tunnel_color, mpc_tunnel_linestyle, mpc_tunnel_linewidth = 'r', '--', 1
+rhrp_color, rhrp_linestyle, rhrp_linewidth = 'g', '-', 2
+
+# ------------------------------- #
+
+# dict {scene_id: x0s}
+scene_x0_mapping = {8: [[6, 0, np.pi/2], [3, 0, np.pi/2], [8, 0, np.pi/2], [4, -2, np.pi/2], [8, 2, np.pi/2]],
+                    10: [[-2.5, 1.5, 0.], [2.4, -5.6, 0.], [3, .5, 0.], [2, 2.5, 0.], [-5, 1.5, 0.], [-2.8, -3.8, 0.]],
+                    12: [[-1, 5, 0], [-4, -5.7, 0], [7, 0.25, 0], [1.2, 1.2, 0], [-3, 3, 0], [3.5, 3.5, 0]],
+                    13: [[0, -5, np.pi/2], [-2, -5, np.pi/2], [2, -5, np.pi/2], [-4, -5, np.pi/2], [4, -5, np.pi/2]],
+                    22: [[0, -5, np.pi/2], [-2, -5, np.pi/2], [2, -5, np.pi/2], [-4, -5, np.pi/2], [4, -5, np.pi/2]]}
+x0s = np.array(scene_x0_mapping[scene_id])
+
+if robot_type_id == 0:
+    x0s = x0s[:, :2]
+
+N = x0s.shape[0]
+
+travelled_path_handles = [[]] * N
+robot_handles = [[]] * N
+controllers = [[]] * N
+scenes = [[]] * N
+
+assert gui_robot_idx < N
+scenes[gui_robot_idx], robot, controllers[gui_robot_idx], _ = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity)
+if scene_id == 8:  # Special fix to show all initial positions in this scenario
+    scenes[gui_robot_idx].xlim[1] += 2
+if show_mpc_tunnel == 0:
+    mpc_tunnel_color, rhrp_color = 'None', 'None'
+if show_mpc_sol == 0:
+    mpc_sol_color = 'None'
+if show_travelled_path == 0:
+    travelled_path_color = 'None'
+gui = PFMPCGUI(robot, scenes[gui_robot_idx], x0s[gui_robot_idx], scenes[gui_robot_idx].xlim, scenes[gui_robot_idx].ylim,
+               controller=controllers[gui_robot_idx], robot_alpha=1., robot_color=gui_robot_color, robot_markersize=10,
+               reference_color='y', reference_marker='*', reference_markersize=10,
+               pg_color='y', pg_markersize=10, pg_marker='*',
+               theta_pos_color='c', theta_pos_marker='o', theta_pos_markersize=2,
+               obstacles_star_alpha=0.2, obstacles_star_show_reference=0, obstacles_star_color='b',
+               workspace_rho_color='b', workspace_rho_alpha=0.2, indicate_sbc=0,
+               s1_pos_color='None',
+               mpc_path_color=mpc_sol_color, mpc_path_linestyle=mpc_sol_linestyle, mpc_path_linewidth=mpc_sol_linewidth,
+               mpc_tunnel_color=mpc_tunnel_color, mpc_tunnel_linestyle=mpc_tunnel_linestyle, mpc_tunnel_linewidth=mpc_tunnel_linewidth,
+               travelled_path_color=travelled_path_color, travelled_path_linestyle=travelled_path_linestyle, travelled_path_linewidth=travelled_path_linewidth,
+               receding_path_color=rhrp_color, receding_path_linestyle=rhrp_linestyle, receding_path_linewidth=rhrp_linewidth,
+               show_time=1, show_timing=0, show_axis=0
+               )
+
+for i in range(N):
+    if i == gui_robot_idx:
+        continue
+    scenes[i], _, controllers[i], _ = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=robot_type_id, scene_id=scene_id, verbosity=verbosity)
+    robot_handles[i], _ = robot.draw(x0s[i, :], ax=gui.ax, markersize=10, color=robot_color, alpha=1)
+    travelled_path_handles[i] = gui.ax.plot([], [], c=travelled_path_color, ls=travelled_path_linestyle, lw=travelled_path_linewidth, zorder=0)[0]
+
+# Initialize simulation
+ctrl_sim_dt_ratio = int(controllers[0].params['dt'] / dt)
+gui_sim_dt_ratio = int(dt_gui / dt)
+k = 0
+tmp_hs = []
+travelled_path = x0s[:, :2]
+xs = x0s.copy()
+converged = [False] * N
+collision = False
+
+# Init video writing
+if make_video:
+    video_name = input("Video file name: ")
+    video_writer = VideoWriter(video_name, 1/dt_gui)
+    gui.paused = False
+    frame_cntr = 0
+else:
+    # Init gui plot
+    gui.update(xs[gui_robot_idx, :], k*dt)
+    plt.pause(0.005)
+
+
+while gui.fig_open and k*dt <= T_max and not all(converged) and not collision:
+    if gui.paused and not gui.step_once:
+        gui.fig.waitforbuttonpress()
+
+    # Move obstacles
+    for i in range(N):
+        scenes[i].step(dt, robot.h(xs[i, :]))
+
+    # Control update
+    if k % ctrl_sim_dt_ratio == 0:
+        gui.step_once = False
+        for i in range(N):
+            controllers[i].update_policy(xs[i, :], scenes[i].obstacles, workspace=scenes[i].workspace)
+
+
+    # GUI update
+    if k % gui_sim_dt_ratio == 0:
+        [h.remove() for h in tmp_hs if not None]
+        tmp_hs = []
+        if k % ctrl_sim_dt_ratio == 0:
+            gui.update(xs[gui_robot_idx, :], k*dt, controllers[gui_robot_idx], controllers[gui_robot_idx].compute_u(xs[gui_robot_idx, :]))
+        else:
+            gui.update(xs[gui_robot_idx, :], k*dt)
+        for i in range(N):
+            if i == gui_robot_idx:
+                continue
+            robot.update_plot(xs[i, :], robot_handles[i])
+            travelled_path_handles[i].set_data(travelled_path[i, ::2], travelled_path[i, 1::2])
+        if show_mpc_tunnel == 2 or show_mpc_sol == 2:
+            for i in range(N):
+                if i == gui_robot_idx:
+                    continue
+                c = controllers[i]
+                x_sol, s_sol, e_sol = c.mpc.sol2state(c.solution, xs[i, :], c.path_pol)
+                if show_mpc_tunnel == 2:
+                    e_max = c.rho - c.epsilon
+                    rhrp = np.array([pol2pos(c.path_pol, s, c.mpc.build_params['n_pol']) for s in s_sol])
+                    tunnel_polygon = shapely.geometry.LineString(list(zip(rhrp[:, 0], rhrp[:, 1]))).buffer(e_max)
+                    tmp_hs += gui.ax.plot(rhrp[:, 0], rhrp[:, 1], c=rhrp_color, ls=rhrp_linestyle, lw=rhrp_linewidth)
+                    tmp_hs += gui.ax.plot(*tunnel_polygon.exterior.xy, c=mpc_tunnel_color, ls=mpc_tunnel_linestyle, lw=mpc_tunnel_linewidth)
+                if show_mpc_sol == 2:
+                    mpc_path = np.array([robot.h(x_sol[k * robot.nx:(k + 1) * robot.nx]) for k in range(c.params['N'] + 1)])
+                    tmp_hs += gui.ax.plot(*mpc_path.T, c=mpc_sol_color, ls=mpc_sol_linestyle, lw=mpc_sol_linewidth)
+
+        if make_video:
+            video_writer.add_frame(gui.fig, frame_cntr)
+            frame_cntr += 1
+            print("[VideoWriter] wrote frame at time {:.2f}/{:.2f}".format(k * dt, T_max))
+        else:
+            plt.pause(0.005)
+
+    # Robot integration
+    for i in range(N):
+        if converged[i]:
+            u = np.zeros(robot.nu)
+        else:
+            u = controllers[i].compute_u(xs[i, :])
+        xs[i, :], _ = robot.move(xs[i, :], u, dt)
+    travelled_path = np.hstack((travelled_path, xs[:, :2]))
+
+    # Convergence and collision check
+    for i in range(N):
+        converged[i] = controllers[i].theta >= controllers[i].theta_g and np.linalg.norm(robot.h(xs[i, :])-scenes[i].reference_path[-1]) < controllers[i].params['convergence_tolerance']
+        if any([o.interior_point(robot.h(xs[i, :])) for o in scenes[i].obstacles]):
+            collision = True
+            print("Collision")
+
+    k += 1
+
+
+if make_video:
+    # close video writer
+    video_writer.release()
+    print("Finished")
+    fig_open = False
+else:
+    gui.update(xs[gui_robot_idx, :], k*dt)
+    gui.ax.set_title("Time: {:.1f} s. Finished".format(k*dt))
+
+    # Wait until figure closed when converged
+    plt.show()
diff --git a/python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py b/python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py
new file mode 100755
index 0000000..34cc437
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/scripts/test_soads.py
@@ -0,0 +1,161 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from config.load_config import load_config
+from motion_control.soads import draw_vector_field, compute_weights
+
+ctrl_param_file = 'soads_ctrl_params.yaml'
+scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=0, verbosity=3)
+
+# Initialize
+T_max = 30
+dt = controller.params['dt']
+K = int(T_max / dt)
+u = np.zeros((robot.nu, K))
+x = np.zeros((x0.size, K+1))
+x[:, 0] = x0
+pg = scene.reference_path[-1]
+timing_history = {'obstacle': [], 'control': []}
+previous_path = []
+convergence_threshold = 0.01
+converged = False
+paused = True
+step_once = False
+i = 0
+
+# Init plotting
+fig_scene, ax_scene = plt.subplots()
+ax_scene.set_xlabel('x1 [m]', fontsize=15)
+ax_scene.set_ylabel('x2 [m]', fontsize=15)
+travelled_path_handle = ax_scene.plot([], [], 'k-', linewidth=2)[0]
+goal_handle = ax_scene.plot(*pg, 'g*', markersize=16)[0]
+obstacle_handles, _ = scene.init_plot(ax=ax_scene, draw_p0=0, draw_ref=0, show_obs_name=1)
+scene.update_plot(obstacle_handles)
+robot.width = 0
+robot_handles, _ = robot.init_plot(ax=ax_scene, color='y', alpha=1, markersize=16)
+robot.update_plot(x0, robot_handles)
+obstacle_star_handles = []
+color_list = plt.cm.gist_ncar(np.linspace(0, 1, len(scene.obstacles)))
+
+streamplot_handle = None
+
+fig_open = True
+
+def on_close(event):
+    global fig_open
+    fig_open = False
+
+def on_press(event):
+    global paused, step_once, streamplot_handle
+    if event.key == ' ':
+        paused = not paused
+    elif event.key == 'right':
+        step_once = True
+        paused = True
+    elif event.key == 't':
+        fig_timing, ax = plt.subplots()
+        ax.plot(timing_history['obstacle'], '-o', label='obstacle')
+        ax.plot(timing_history['control'], '-o', label='control')
+        fig_timing.canvas.draw()
+    elif event.key == 'w':
+        ax_scene.axis('off')
+        ax_scene.title.set_visible(False)
+        file_name = input("-------------\nFile name: ")
+        fig_scene.savefig("utils/" + file_name, transparent=True)
+        ax_scene.axis('on')
+        ax_scene.title.set_visible(True)
+    elif event.key == 'a':
+        n = int(input("-------------\nStreamplot resolution: "))
+        streamplot_handle = draw_vector_field(pg, controller.obstacles_star, ax_scene, workspace=scene.workspace, n=n, color='orange')
+    else:
+        print(event.key)
+
+def remove_local_plots():
+    global streamplot_handle
+    # Remove streamplot
+    if streamplot_handle is not None:
+        from matplotlib.patches import FancyArrowPatch
+        streamplot_handle.lines.remove()
+        for art in ax_scene.get_children():
+            if not isinstance(art, FancyArrowPatch):
+                continue
+            art.remove()  # Method 1
+        streamplot_handle = None
+
+fig_scene.canvas.mpl_connect('close_event', on_close)
+fig_scene.canvas.mpl_connect('key_press_event', on_press)
+
+ls = []
+
+while fig_open and not converged:
+
+    if i < K and (not paused or step_once):
+        p = robot.h(x[:, i])
+        step_once = False
+        # Move obstacles
+        scene.step(dt, p)
+        # Compute mpc
+        u[:, i] = controller.compute_u(x[:, i], pg, scene.obstacles, workspace=scene.workspace)
+        # Integrate robot state with new control signal
+        x[:, i+1], _ = robot.move(x[:, i], u[:, i], dt)
+        # Add timing to history
+        for k in timing_history.keys():
+            timing_history[k] += [controller.timing[k]]
+
+        # Update plots
+        robot.update_plot(x[:, i], robot_handles)
+        scene.update_plot(obstacle_handles)
+        [h.remove() for h in obstacle_star_handles if h is not None]
+        obstacle_star_handles = []
+        for j, cl in enumerate(controller.obstacle_clusters):
+            for o in cl.obstacles:
+                lh, _ = o.draw(ax=ax_scene, fc='lightgrey', show_reference=False)
+                obstacle_star_handles += lh
+        for o in controller.obstacles_star:
+            lh, _ = o.draw(ax=ax_scene, fc='red', show_reference=False, alpha=0.9, zorder=0)
+            obstacle_star_handles += lh
+            lh = ax_scene.plot(*o.xr(), 'gd', markersize=8)
+            obstacle_star_handles += lh
+
+        # obstacle_star_handles += [ax_scene.quiver(*scene.boundary.boundary_mapping(x[:, i]), *scene.boundary.normal(x[:, i]))]
+
+        gamma = [obs.distance_function(p) for obs in controller.obstacles_star]
+        w = compute_weights(gamma)
+
+        if ls:
+            [l.remove() for l in ls]
+        ls = []
+        for j, o in enumerate(controller.obstacles_star):
+            ls += [ax_scene.text(*o.xr(), "{:.2f}".format(w[j]))]
+            if o.reference_direction(p).dot(pg-p) > 0 and o.normal(p).dot(pg-p) > 0:
+                ls += ax_scene.plot(*o.xr(), 'r+', ms=12, zorder=10)
+            else:
+                ls += ax_scene.plot(*o.xr(), 'k+', ms=12, zorder=10)
+        #     b = o.boundary_mapping(x[:, i])
+        #     dx_o = f(x[:, i], pg, [o])
+        #     ls += ax_scene.plot(*b, 'yx')
+        #     ls += [ax_scene.quiver(*b, *dx_o, zorder=3, color='y')]
+
+        travelled_path_handle.set_data(x[0, :i+1], x[1, :i+1])
+
+        ax_scene.set_title("Time: {:.1f} s".format(i*dt))
+
+        i += 1
+
+        fig_scene.canvas.draw()
+
+    converged = np.linalg.norm(robot.h(x[:, i])-pg) < convergence_threshold
+
+    if i == K or converged:
+        ax_scene.set_title("Time: {:.1f} s. Finished".format(i * dt))
+        fig_scene.canvas.draw()
+
+    plt.pause(0.005)
+
+
+ot = timing_history['obstacle']
+print("Timing\n-----\nMean: {:.2f}\nMax: {:.2f}\nStdDev: {:.2f}".format(np.mean(ot), np.max(ot), np.std(ot)))
+plt.figure()
+plt.plot(ot)
+
+# Wait until figure closed when converged
+plt.show()
diff --git a/python/ur_simple_control/path_generation/star_navigation/setup.py b/python/ur_simple_control/path_generation/star_navigation/setup.py
new file mode 100644
index 0000000..38cc882
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/setup.py
@@ -0,0 +1,19 @@
+from setuptools import setup, find_packages
+
+setup(name='star_navigation',
+      version='1.0',
+      packages=find_packages(),
+      #      install_requires=[
+      #          'pyyaml',
+      #          'numpy',
+      #          'scipy',
+      #          'matplotlib',
+      #          'shapely',
+      #          #          'casadi',
+      #          'opengen',
+      #          #'opencv-python',
+      #          #'cargo',
+      #          #'rust',
+      #          #          'Tkinter'
+      #      ]
+)
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
new file mode 100644
index 0000000..f45c709
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/PKG-INFO
@@ -0,0 +1,3 @@
+Metadata-Version: 2.1
+Name: star_navigation
+Version: 1.0
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
new file mode 100644
index 0000000..a108d48
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/SOURCES.txt
@@ -0,0 +1,51 @@
+.gitignore
+.gitmodules
+README.md
+setup.py
+config/__init__.py
+config/load_config.py
+config/scene.py
+config/params/dmp_ctrl_params.yaml
+config/params/pfmpc_artificial_reference_params.yaml
+config/params/pfmpc_ds_params.yaml
+config/params/pfmpc_obstacle_constraints_params.yaml
+config/params/robot_params.yaml
+config/params/soads_ctrl_params.yaml
+motion_control/__init__.py
+motion_control/dmp/__init__.py
+motion_control/dmp/dmp.py
+motion_control/pfmpc_artificial_reference/__init__.py
+motion_control/pfmpc_artificial_reference/motion_controller.py
+motion_control/pfmpc_artificial_reference/mpc.py
+motion_control/pfmpc_artificial_reference/mpc_build/.gitignore
+motion_control/pfmpc_ds/__init__.py
+motion_control/pfmpc_ds/motion_controller.py
+motion_control/pfmpc_ds/mpc.py
+motion_control/pfmpc_ds/path_generator.py
+motion_control/pfmpc_ds/workspace_modification.py
+motion_control/pfmpc_ds/mpc_build/.gitignore
+motion_control/pfmpc_obstacle_constraints/__init__.py
+motion_control/pfmpc_obstacle_constraints/motion_controller.py
+motion_control/pfmpc_obstacle_constraints/mpc.py
+motion_control/pfmpc_obstacle_constraints/mpc_build/.gitignore
+motion_control/soads/__init__.py
+motion_control/soads/soads.py
+robot/__init__.py
+robot/bicycle.py
+robot/mobile_robot.py
+robot/omnidirectional.py
+robot/unicycle.py
+scripts/compute_time.py
+scripts/run_simulation.py
+scripts/simulate_several_controllers.py
+scripts/simulate_several_initpos.py
+scripts/test_soads.py
+star_navigation.egg-info/PKG-INFO
+star_navigation.egg-info/SOURCES.txt
+star_navigation.egg-info/dependency_links.txt
+star_navigation.egg-info/top_level.txt
+visualization/__init__.py
+visualization/scene_gui.py
+visualization/video_writer.py
+visualization/figures/.gitignore
+visualization/videos/.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt
new file mode 100644
index 0000000..3242cb5
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/star_navigation.egg-info/top_level.txt
@@ -0,0 +1,4 @@
+config
+motion_control
+robot
+visualization
diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py b/python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py
new file mode 100644
index 0000000..9d7c854
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/visualization/__init__.py
@@ -0,0 +1,2 @@
+from .scene_gui import SceneGUI, StarobsGUI, PFMPCGUI
+from .video_writer import VideoWriter
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore b/python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/visualization/figures/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py b/python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py
new file mode 100644
index 0000000..dad824a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/visualization/scene_gui.py
@@ -0,0 +1,383 @@
+import matplotlib.pyplot as plt
+import shapely
+import numpy as np
+import pathlib
+from motion_control.pfmpc_ds import MotionController as PFMPC_ds
+from motion_control.pfmpc_ds import ControlMode
+from motion_control.pfmpc_obstacle_constraints import MotionController as PFMPC_obstacle_constraints
+from motion_control.pfmpc_artificial_reference import MotionController as PFMPC_artificial_reference
+from motion_control.pfmpc_artificial_reference import pol2pos
+from starworlds.utils.misc import draw_shapely_polygon
+from motion_control.soads import draw_vector_field
+
+
+class SceneGUI:
+
+    def __init__(self, robot, scene, xlim, ylim, show_axis=False,
+                 robot_color='k', robot_markersize=14, robot_alpha=0.7,
+                 reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14,
+                 obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False,
+                 travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2):
+        self.scene = scene
+        self.robot = robot
+        self.fig, self.ax = plt.subplots()
+        if not show_axis:
+            self.ax.set_axis_off()
+        self.scene_handles, _ = self.scene.init_plot(self.ax, obstacle_color=obstacle_color, obstacle_edge_color=obstacle_edge_color, show_obs_name=show_obs_name, draw_p0=0, draw_ref=1, reference_color=reference_color, reference_alpha=reference_alpha, reference_marker=reference_marker, reference_markersize=reference_markersize)
+        self.robot_handles, _ = robot.init_plot(ax=self.ax, color=robot_color, markersize=robot_markersize, alpha=robot_alpha)
+        self.travelled_path_handle = self.ax.plot([], [], color=travelled_path_color, linestyle=travelled_path_linestyle, linewidth=travelled_path_linewidth, zorder=0)[0]
+        self.ax.set_xlim(xlim), self.ax.set_ylim(ylim)
+        # Simulation ctrl
+        self.fig_open = True
+        self.paused = True
+        self.step_once = False
+        self.draw_vector_field = False
+        self.fig.canvas.mpl_connect('close_event', self.on_close)
+        self.fig.canvas.mpl_connect('key_press_event', self.on_press)
+        # Memory
+        self.travelled_path = []
+
+    def update(self, robot_state=None, time=None):
+        if time:
+            self.ax.set_title("Time: {:.1f} s".format(time))
+
+        self.scene.update_plot(self.scene_handles)
+        # Obstacles
+        # [oh[0].update_plot(oh[1]) for oh in zip(self.obstacles, self.obstacle_handles)]
+
+        # Robot and goal position
+        if robot_state is not None:
+            self.travelled_path += list(self.robot.h(robot_state))
+            self.robot.update_plot(robot_state, self.robot_handles)
+            self.travelled_path_handle.set_data(self.travelled_path[::2], self.travelled_path[1::2])
+
+    def on_close(self, event):
+        self.fig_open = False
+
+    def on_press(self, event):
+        if event.key == ' ':
+            self.paused = not self.paused
+        elif event.key == 'right':
+            self.step_once = True
+            self.paused = True
+        # elif event.key == 'f':
+        #     if len(self.scene.reference_path) > 1:
+        #         print("Vector field is only shown for setpoint setup!")
+        #         return
+        #     self.paused = True
+        #     self.step_once = True
+        #     self.draw_vector_field = True
+        #     draw_vector_field(self.scene.reference_path, controller.obstacles_star, gui.ax, workspace=controller.workspace_rho, n=200,
+        #                       color='orange', zorder=-3)
+        elif event.key == 'w':
+            fig_name = input("Figure file name: ")
+            self.save_fig(fig_name)
+        else:
+            print(event.key)
+
+    def save_fig(self, name):
+        figures_path = pathlib.PurePath(__file__).parents[0].joinpath("figures")
+        title = self.ax.get_title()
+        self.ax.set_title("")
+        self.fig.savefig(str(figures_path.joinpath(name + ".png")), bbox_inches='tight')
+        self.ax.set_title(title)
+
+
+class StarobsGUI(SceneGUI):
+    def __init__(self, robot, scene, init_robot_state, xlim, ylim, show_axis=False,
+                 robot_color='k', robot_markersize=14, robot_alpha=0.7,
+                 obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False,
+                 reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=14,
+                 travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2,
+                 obstacles_star_color='r', obstacles_star_show_reference=True, obstacles_star_alpha=0.1,
+                 show_time = True, show_timing = False):
+        self.show_timing = show_timing
+        self.show_time = show_time
+        self.obstacle_star_handles = []
+        self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference,
+                                            'alpha': obstacles_star_alpha, 'zorder': 0}
+        # super().__init__(robot, scene, xlim, ylim, show_axis, robot_color, robot_markersize,
+        #                  robot_alpha, obstacle_color, obstacle_edge_color, show_obs_name, goal_color, goal_marker,
+        #                  goal_markersize, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth)
+        super().__init__(robot, scene, xlim, ylim, show_axis, robot_color, robot_markersize,
+                         robot_alpha, reference_color, reference_alpha, reference_marker, reference_markersize,
+                         obstacle_color, obstacle_edge_color, show_obs_name, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth)
+
+        super().update(init_robot_state, 0)
+
+    def update(self, robot_state=None, time=None, controller=None, u=None):
+        # SceneFig update
+        super().update(robot_state)
+
+        title = "Time: {:.1f} s".format(time) if self.show_time else ""
+        # if self.show_timing:
+        #     if controller is None:
+        #         prev_title = self.ax.get_title()
+        #         time_timing = prev_title.split('\n')
+        #         title += "\n" + time_timing[-1]
+        #     else:
+        #         title += "\nCompute Time ({:.1f} / {:.1f} / {:.1f})".format(controller.timing['workspace'],
+        #                                                                     controller.timing['target'],
+        #                                                                     controller.timing['mpc'])
+        self.ax.set_title(title)
+
+        if controller is None:
+            return
+
+        # Star obstacles
+        [h.remove() for h in self.obstacle_star_handles if h is not None]
+        self.obstacle_star_handles = []
+        if controller.obstacles_star is not None:
+            for o in controller.obstacles_star:
+                lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options)
+                self.obstacle_star_handles += lh
+
+class PFMPCGUI(SceneGUI):
+
+    def __init__(self, robot, scene, init_robot_state, xlim, ylim, show_axis=False,
+                 robot_color='c', robot_markersize=10, robot_alpha=1,
+                 obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False,
+                 reference_color='y', reference_alpha=1, reference_marker='*', reference_markersize=10,
+                 travelled_path_color='k', travelled_path_linestyle='--', travelled_path_linewidth=2,
+                 theta_pos_color='y', theta_pos_marker='*', theta_pos_markersize=10,
+                 s1_pos_color='None', s1_pos_marker='+', s1_pos_markersize=10,
+                 pg_color='b', pg_marker='*', pg_markersize=10,
+                 receding_path_color='g', receding_path_linestyle='-', receding_path_linewidth=1, receding_path_marker=None,
+                 mpc_path_color='k', mpc_path_linestyle='-', mpc_path_linewidth=1,
+                 base_sol_color='None', base_sol_linestyle=':', base_sol_linewidth=1,
+                 mpc_artificial_path_color='y', mpc_artificial_path_linestyle='-.', mpc_artificial_path_linewidth=4,
+                 mpc_tunnel_color='r', mpc_tunnel_linestyle='--', mpc_tunnel_linewidth=1, mpc_tunnel_alpha=1,
+                 obstacles_star_color='b', obstacles_star_show_reference=False, obstacles_star_alpha=0.2,
+                 workspace_rho_color='None', workspace_rho_show_reference=False, workspace_rho_alpha=0.2,
+                 workspace_horizon_color='tab:blue', workspace_horizon_linestyle='--', workspace_horizon_linewidth=1,
+                 show_u_history=False, u_history_color='k', u_history_marker='o',
+                 show_mpc_solution=False, show_mpc_cost=False, controller=None,
+                 indicate_sbc=True, sbc_color='r',
+                 show_time=True, show_timing=False):
+        self.show_timing = show_timing
+        self.show_time = show_time
+        self.indicate_sbc = indicate_sbc
+        self.robot_color, self.sbc_color = robot_color, sbc_color
+        self.obstacle_star_handles = []
+        self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference,
+                                            'alpha': obstacles_star_alpha, 'zorder': 0}
+        self.workspace_rho_draw_options = {'ec': workspace_rho_color, 'fc': 'None', 'ls': '--',
+                                           'show_reference': workspace_rho_show_reference,
+                                           'alpha': workspace_rho_alpha, 'zorder': 0}
+        super().__init__(robot, scene, xlim, ylim, show_axis, robot_color, robot_markersize,
+                         robot_alpha, reference_color, reference_alpha, reference_marker, reference_markersize, obstacle_color, obstacle_edge_color, show_obs_name, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth)
+        self.theta_pos_handle = self.ax.plot([], [], color=theta_pos_color, marker=theta_pos_marker, markersize=theta_pos_markersize, zorder=0)[0]
+        self.pg_handle = self.ax.plot([], [], color=pg_color, marker=pg_marker, markersize=pg_markersize)[0]
+        self.receding_path_handle = self.ax.plot([], [], color=receding_path_color, linestyle=receding_path_linestyle, linewidth=receding_path_linewidth, marker=receding_path_marker, zorder=0)[0]
+        self.mpc_path_handle = self.ax.plot([], [], color=mpc_path_color, linestyle=mpc_path_linestyle, linewidth=mpc_path_linewidth, zorder=0)[0]
+        self.s1_pos_handle = self.ax.plot([], [], color=s1_pos_color, marker=s1_pos_marker, markersize=s1_pos_markersize, zorder=0)[0]
+        # Tunnel and base_solution for PFMPC_DS
+        self.mpc_tunnel_handle = self.ax.plot([], [], color=mpc_tunnel_color, linestyle=mpc_tunnel_linestyle, linewidth=mpc_tunnel_linewidth, alpha=mpc_tunnel_alpha, zorder=0)[0]
+        self.base_sol_path_handle = self.ax.plot([], [], color=base_sol_color, linestyle=base_sol_linestyle, linewidth=base_sol_linewidth, zorder=0, dashes=(0.8, 0.8))[0]
+
+        self.workspace_horizon_handle = self.ax.plot([], [], color=workspace_horizon_color, linestyle=workspace_horizon_linestyle, linewidth=workspace_horizon_linewidth)[0]
+
+        # Artifical path for PFMPC_artificial
+        self.mpc_artificial_path_handle = self.ax.plot([], [], color=mpc_artificial_path_color, linestyle=mpc_artificial_path_linestyle, linewidth=mpc_artificial_path_linewidth, zorder=4)[0]
+
+        self.show_mpc_solution = show_mpc_solution
+        self.show_mpc_cost = show_mpc_cost
+        self.show_u_history = show_u_history
+        if show_u_history:
+            self.fig_u_history, self.ax_u_history = plt.subplots(self.robot.nu, 1)
+            self.u_history = []
+            self.u_history_handles = []
+            self.u_infeasible = []
+            self.u_infeasible_handles = []
+            for i in range(self.robot.nu):
+                self.u_history_handles += self.ax_u_history[i].plot([], [], color=u_history_color, marker=u_history_marker)
+                self.u_infeasible_handles += self.ax_u_history[i].plot([], [], color='r', marker=u_history_marker, linestyle="None")
+                u_span = self.robot.u_max[i] - self.robot.u_min[i]
+                self.ax_u_history[i].set_ylim([self.robot.u_min[i]-0.1*u_span, self.robot.u_max[i]+0.1*u_span])
+                self.ax_u_history[i].plot([0, 1e10], [robot.u_min[i], robot.u_min[i]], 'r--')
+                self.ax_u_history[i].plot([0, 1e10], [robot.u_max[i], robot.u_max[i]], 'r--')
+
+        if show_mpc_solution:
+            N = controller.mpc.build_params['N']
+            self.fig_mpc_solution, self.ax_mpc_solution = plt.subplots(2, 3)
+            self.s_handle = self.ax_mpc_solution[0, 0].plot(np.linspace(1, N, N), [None] * N, '-o')[0]
+            self.lam_rho_handle = self.ax_mpc_solution[0, 0].plot([0, N], [None, None], 'r--')[0]
+            self.ax_mpc_solution[0, 0].plot([0, N], [N, N], 'r--')
+            self.ax_mpc_solution[0, 0].set_ylim(0, 1.1*N)
+            self.ax_mpc_solution[0, 0].set_title('Path coordinate, s')
+            self.rho_handle = self.ax_mpc_solution[1, 0].plot([0, N], [None, None], 'k--')[0]
+            self.emax_handle = self.ax_mpc_solution[1, 0].plot([0, N], [None, None], 'r--')[0]
+            self.e_handle = self.ax_mpc_solution[1, 0].plot(np.linspace(0, N, N+1, '-o'), [None] * (N+1), '-o')[0]
+            self.mpc_artificial_error_handle = self.ax_mpc_solution[1, 0].plot(np.linspace(0, N, N+1, '-o'), [None] * (N+1), 'g-o')[0]
+
+            self.ax_mpc_solution[1, 0].set_xlim(0, N)
+            if hasattr(controller, 'rho0'):
+                self.ax_mpc_solution[1, 0].set_ylim(0, 1.5*controller.params['rho0'])
+            self.ax_mpc_solution[1, 0].set_title('Tracking error')
+            # Assumes 2 control signals
+            self.u1_handle = self.ax_mpc_solution[0, 1].plot(np.linspace(-1, N-1, N+1), [None] * (N + 1), '-o')[0]
+            self.ax_mpc_solution[0, 1].plot([0, N], [robot.u_min[0], robot.u_min[0]], 'r--')
+            self.ax_mpc_solution[0, 1].plot([0, N], [robot.u_max[0], robot.u_max[0]], 'r--')
+            self.ax_mpc_solution[0, 1].set_title('u1')
+            self.u2_handle = self.ax_mpc_solution[1, 1].plot(np.linspace(-1, N-1, N+1), [None] * (N + 1), '-o')[0]
+            self.ax_mpc_solution[1, 1].plot([0, N], [robot.u_min[1], robot.u_min[1]], 'r--')
+            self.ax_mpc_solution[1, 1].plot([0, N], [robot.u_max[1], robot.u_max[1]], 'r--')
+            self.ax_mpc_solution[1, 1].set_title('u2')
+            if show_mpc_cost:
+                # draw the initial pie chart
+                self.cost_ax_handle = self.ax_mpc_solution[0, 2]
+                self.cost_u_ax_handle = self.ax_mpc_solution[1, 2]
+
+        plt.figure(self.fig)
+
+        super().update(init_robot_state, 0)
+        # self.update(init_robot_state, None, 0)
+
+    def update(self, robot_state, time, controller=None, u=None):
+        # SceneFig update
+        super().update(robot_state)
+
+        title = "Time: {:.1f} s".format(time) if self.show_time else ""
+        if self.show_timing:
+            if controller is None:
+                prev_title = self.ax.get_title()
+                time_timing = prev_title.split('\n')
+                title += "\n" + time_timing[-1]
+            else:
+                title += "\nCompute Time ({:.1f} / {:.1f} / {:.1f})".format(controller.timing['workspace'],
+                                                                            controller.timing['target'],
+                                                                            controller.timing['mpc'])
+        self.ax.set_title(title)
+
+        if controller is None:
+            return
+
+        if isinstance(controller, PFMPC_ds):
+            if controller.pg is None:
+                self.pg_handle.set_data([],[])
+            else:
+                self.pg_handle.set_data(*controller.pg)
+            # Star environment
+            [h.remove() for h in self.obstacle_star_handles if h is not None]
+            self.obstacle_star_handles = []
+            for o in controller.obstacles_star:
+                lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options)
+                self.obstacle_star_handles += lh
+            if self.scene.workspace is not None:
+                # lh, _ = workspace.draw(ax=self.ax, **self.workspace_rho_draw_options)
+                # self.obstacle_star_handles += lh
+                # lh, _ = controller.workspace_rho.draw(ax=self.ax, fc='w', zorder=-8, show_reference=0)
+                lh, _ = controller.workspace_rho.draw(ax=self.ax, **self.workspace_rho_draw_options)
+                self.obstacle_star_handles += lh
+            if controller.params['workspace_horizon'] > 0:
+                self.workspace_horizon_handle.set_data(*shapely.geometry.Point(self.robot.h(robot_state)).buffer(controller.params['workspace_horizon']).exterior.xy)
+
+            if self.indicate_sbc and controller.mode == ControlMode.SBC:
+                self.robot_handles[0].set_color(self.sbc_color)
+            else:
+                self.robot_handles[0].set_color(self.robot_color)
+
+            # SBC base solution
+            if controller.sol_feasible:
+                self.base_sol_path_handle.set_data([], [])
+            else:
+                x_sol_base, _, _ = controller.mpc.sol2state(controller.mpc.base_solution(robot_state, controller.path_pol, 0), robot_state, controller.path_pol)
+                base_sol_path = np.array(
+                    [self.robot.h(x_sol_base[k * self.robot.nx:(k + 1) * self.robot.nx]) for k in range(controller.params['N'] + 1)])
+                self.base_sol_path_handle.set_data(base_sol_path[:, 0], base_sol_path[:, 1])
+
+        # RHRP
+        self.receding_path_handle.set_data(controller.rhrp_path[:, 0], controller.rhrp_path[:, 1])
+        if controller.reference_path.geom_type == 'LineString':
+            self.theta_pos_handle.set_data(*controller.reference_path.interpolate(controller.theta).coords[0])
+
+        if controller.sol_feasible:
+            if isinstance(controller, PFMPC_ds):
+                u_sol, ds_sol = controller.mpc.sol2uds(controller.solution)
+                x_sol, s_sol, e_sol = controller.mpc.sol2state(controller.solution, robot_state, controller.path_pol)
+                # Draw tunnel
+                e_max = controller.rho - controller.epsilon
+                tunnel_polygon = shapely.geometry.LineString(
+                    list(zip(controller.rhrp_path[:, 0], controller.rhrp_path[:, 1]))).buffer(
+                    e_max)
+                if tunnel_polygon.geom_type == 'Polygon':
+                    self.mpc_tunnel_handle.set_data(*tunnel_polygon.exterior.xy)
+                else:
+                    print("[SceneFig]: Tunnel polygon not polygon.")
+
+            if isinstance(controller, PFMPC_obstacle_constraints):
+                u_sol, ds_sol = controller.mpc.sol2uds(controller.solution)
+                x_sol, s_sol, e_sol = controller.mpc.sol2state(controller.solution, robot_state, controller.path_pol)
+
+            if isinstance(controller, PFMPC_artificial_reference):
+                xa0, u_sol, ua_sol, ds_sol = controller.mpc.sol2xa0uuaw(controller.solution)
+                x_sol, s_sol, xa_sol = controller.mpc.sol2state(controller.solution, robot_state, controller.path_pol)
+                ref_sol = np.array([pol2pos(controller.path_pol, s, controller.mpc.build_params['n_pol']) for s in s_sol])
+                mpc_path = np.array([self.robot.h(x_sol[k * self.robot.nx:(k + 1) * self.robot.nx]) for k in
+                                     range(controller.params['N'] + 1)])
+                mpc_artificial_path = np.array([self.robot.h(xa_sol[k * self.robot.nx:(k + 1) * self.robot.nx])
+                                                for k in range(controller.params['N'] + 1)])
+                e_sol = np.linalg.norm(ref_sol - mpc_path, axis=1)
+                self.mpc_artificial_path_handle.set_data(mpc_artificial_path[:, 0], mpc_artificial_path[:, 1])
+                self.receding_path_handle.set_data(ref_sol[:, 0], ref_sol[:, 1])
+
+            # print(x_sol)
+            mpc_path = np.array([self.robot.h(x_sol[k * self.robot.nx:(k + 1) * self.robot.nx]) for k in range(controller.params['N'] + 1)])
+            self.mpc_path_handle.set_data(mpc_path[:, 0], mpc_path[:, 1])
+            self.s1_pos_handle.set_data(*pol2pos(controller.path_pol, s_sol[1], controller.mpc.build_params['n_pol']))
+
+
+        if self.show_u_history:
+            self.u_infeasible += u if not controller.sol_feasible else [None] * self.robot.nu
+            self.u_history += u
+            for i in range(self.robot.nu):
+                if len(self.u_history) > 2:
+                    N = len(self.u_history) // self.robot.nu
+                    self.u_history_handles[i].set_data(np.arange(0, N), self.u_history[i::self.robot.nu])
+                    self.u_infeasible_handles[i].set_data(np.arange(0, N), self.u_infeasible[i::self.robot.nu])
+                    self.ax_u_history[i].set_xlim([0, N])
+
+        # MPC solution plot
+        if self.show_mpc_solution:
+            sol_color = 'tab:blue' if controller.sol_feasible else 'tab:brown'
+            self.s_handle.set_ydata(np.array(s_sol[1:]) / (controller.params['dt'] * controller.mpc.build_params['w_max']))
+            self.s_handle.set_color(sol_color)
+            self.e_handle.set_ydata(e_sol)
+            self.e_handle.set_color(sol_color)
+            if isinstance(controller, PFMPC_ds):
+                self.lam_rho_handle.set_ydata(controller.lam_rho * np.ones(2))
+                self.emax_handle.set_ydata([e_max, e_max])
+                self.rho_handle.set_ydata([controller.rho, controller.rho])
+            if isinstance(controller, PFMPC_artificial_reference):
+                ea_sol = np.linalg.norm(mpc_path - mpc_artificial_path, axis=1)
+                self.mpc_artificial_error_handle.set_ydata(ea_sol)
+            self.u1_handle.set_ydata([controller.u_prev[0]] + u_sol[::2])
+            self.u2_handle.set_ydata([controller.u_prev[1]] + u_sol[1::2])
+            self.u1_handle.set_color(sol_color)
+            self.u2_handle.set_color(sol_color)
+            if self.show_mpc_cost:
+                if isinstance(controller, PFMPC_ds):
+                    cost = controller.mpc.sol2cost(controller.solution, robot_state, controller.path_pol, controller.params, controller.u_prev)
+                    tot_cost = cost['s'] + cost['e'] + cost['u'] + cost['ud']
+                    print("cost: {:.3f}.  s: {:2.0f}%, e: {:2.0f}%, u: {:2.0f}%, ud: {:2.0f}%".format(tot_cost, 100*cost['s']/tot_cost, 100*cost['e']/tot_cost, 100*cost['u']/tot_cost, 100*cost['ud']/tot_cost))
+
+                if isinstance(controller, PFMPC_artificial_reference):
+
+                    o_par = controller.extract_obs_par(self.obstacles).copy()
+                    # Add safety margin in constraints
+                    for i in range(controller.mpc.build_params['max_No_ell']):
+                        o_par[6 * i + 4] += 0.2
+                        o_par[6 * i + 5] += 0.2
+                    print(controller.mpc.sol2cost(controller.solution, robot_state, controller.path_pol, o_par, controller.params))
+                    # print(x_sol)
+                    # print(xa_sol)
+                    # print(u_sol)
+                    # print(ua_sol)
+                    # print(ds_sol)
+            # if self.show_mpc_cost and cost is not None:
+            #     cost_dict = dict(list(cost.items())[:-4])
+            #     u_cost_dict = dict(list(cost.items())[-4:])
+            #     self.cost_ax_handle.clear()
+            #     self.cost_ax_handle.pie(cost_dict.values(), labels=cost_dict.keys(), wedgeprops=dict(width=0.5))
+            #     self.cost_u_ax_handle.clear()
+            #     self.cost_u_ax_handle.pie(u_cost_dict.values(), labels=u_cost_dict.keys(), wedgeprops=dict(width=0.5))
diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py b/python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py
new file mode 100644
index 0000000..1124cb1
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/visualization/video_writer.py
@@ -0,0 +1,45 @@
+import matplotlib.pyplot as plt
+from matplotlib.backends.backend_agg import FigureCanvasAgg as FigureCanvas
+import cv2
+import numpy as np
+import pathlib
+import os
+import subprocess
+import glob
+
+class VideoWriter:
+
+    def __init__(self, file_name, framerate):
+        self.file_name = file_name
+        self.video = None
+        self.framerate = framerate
+        self.videos_path = str(pathlib.PurePath(pathlib.Path(__file__).parents[0].joinpath("videos")))
+
+    def add_frame(self, fig, k):
+        fig.savefig(str(pathlib.PurePath(self.videos_path, self.file_name + '%02d.png' % k)))
+        return None
+
+        fig.set_size_inches(16, 9)
+        fig.tight_layout()
+        canvas = FigureCanvas(fig)
+        canvas.draw()
+        mat = np.array(canvas.renderer._renderer)
+        mat = cv2.cvtColor(mat, cv2.COLOR_RGB2BGR)
+        if self.video is None:
+            file_path = str(pathlib.PurePath(pathlib.Path(__file__).parents[0].joinpath("videos"), self.file_name + '.mp4'))
+            self.video = cv2.VideoWriter(file_path, cv2.VideoWriter_fourcc(*'mp4v'), self.framerate, (mat.shape[1], mat.shape[0]))
+
+        # write frame to video
+        self.video.write(mat)
+
+    def release(self):
+        os.chdir(self.videos_path)
+        subprocess.call([
+            'ffmpeg', '-i', self.file_name + '%02d.png', '-framerate', str(self.framerate), '-pix_fmt', 'yuv420p',
+            self.file_name+'.mp4'
+        ])
+        for file_name in glob.glob("*.png"):
+            os.remove(file_name)
+        return None
+        cv2.destroyAllWindows()
+        self.video.release()
diff --git a/python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore b/python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/star_navigation/visualization/videos/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc b/python/ur_simple_control/path_generation/starworld_tunnel_mpc
deleted file mode 160000
index 94c3d55..0000000
--- a/python/ur_simple_control/path_generation/starworld_tunnel_mpc
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 94c3d55671c9c57bcfc2509d6f8a64f26217861e
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md
new file mode 100644
index 0000000..5c5ae97
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/README.md
@@ -0,0 +1,22 @@
+# Starworld tunnel-MPC
+
+
+## Installation
+Clone repo and initialize submodule
+```
+git clone https://github.com/albindgit/starworld_tunnel_mpc.git
+cd starworld_tunnel_mpc/starworlds
+git submodule init
+git submodule update
+cd ..
+```
+Create and activate virtual environment
+```
+python -m venv venv
+. venv/bin/activate
+```
+Install package
+```
+pip install -e .
+pip install -e starworlds
+```
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py
new file mode 100644
index 0000000..b38df1d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/load_config.py
@@ -0,0 +1,79 @@
+import yaml
+import pathlib
+from config.scene import Scene, scene_description
+from motion_control.soads import SoadsController
+from motion_control.tunnel_mpc import TunnelMpcController
+from motion_control.tunnel_mpc_convergence import TunnelMpcController as TunnelMpcControllerConvergence
+from robot import Unicycle, Omnidirectional, Bicycle
+import numpy as np
+
+
+def load_config(scene_id=None, robot_type_id=None, ctrl_param_file=None, all_convex_obstacles=False, verbosity=0):
+    # Scene init
+    if scene_id is None:
+        print("Select scene ID\n -------------")
+        for i, description in scene_description.items():
+            print(str(i) + ": " + description)
+        scene_id = int(input("-------------\nScene ID: "))
+    scene = Scene(scene_id, all_convex=all_convex_obstacles)
+
+    param_path = pathlib.PurePath(__file__).parents[0].joinpath('params')
+
+    # Load robot
+    robot_param_path = str(param_path.joinpath('robot_params.yaml'))
+    with open(r'' + robot_param_path) as stream:
+        params = yaml.safe_load(stream)
+    if robot_type_id is None:
+        print("Select robot ID\n -------------")
+        for i, k in enumerate(params.keys()):
+            print(str(i) + ": " + k)
+        robot_type_id = int(input("-------------\nRobot type: "))
+    robot_type = list(params.keys())[robot_type_id]
+    robot_params = params[robot_type]
+    if robot_params['model'] == 'Omnidirectional':
+        robot = Omnidirectional(width=robot_params['width'],
+                                vel_max=robot_params['vel_max'],
+                                name=robot_type)
+        x0 = scene.p0
+    elif robot_params['model'] == 'Unicycle':
+        robot = Unicycle(width=robot_params['width'], vel_min=[robot_params['lin_vel_min'], -robot_params['ang_vel_max']],
+                         vel_max=[robot_params['lin_vel_max'], robot_params['ang_vel_max']],
+                         name=robot_type)
+        try:
+            x0 = np.append(scene.p0, [scene.theta0])
+        except AttributeError:
+            x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])])
+    elif robot_params['model'] == 'Bicycle':
+        robot = Bicycle(width=robot_params['width'],
+                        vel_min=[robot_params['lin_vel_min'], -robot_params['steer_vel_max']],
+                        vel_max=[robot_params['lin_vel_max'], robot_params['steer_vel_max']],
+                        steer_max=robot_params['steer_max'],
+                        name=robot_type)
+        try:
+            x0 = np.append(scene.p0, [scene.theta0, 0])
+            # x0 = np.append(scene.p0, [scene.theta0])
+        except AttributeError:
+            x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0]), 0])
+            # x0 = np.append(scene.p0, [np.arctan2(scene.pg[1]-scene.p0[1], scene.pg[0]-scene.p0[0])])
+    else:
+        raise Exception("[Load Config]: Invalid robot model.\n"
+                        "\t\t\tSelection: {}\n"
+                        "\t\t\tValid selections: [Omnidirectional, Unicycle, Bicycle]".format(robot_params['model']))
+
+    # Load control parameters
+    ctrl_param_path = str(param_path.joinpath(ctrl_param_file))
+    # param_file = str(pathlib.PurePath(pathlib.Path(__file__).parent, ctrl_param_file))
+    with open(r'' + ctrl_param_path) as stream:
+        params = yaml.safe_load(stream)
+    if 'soads' in params:
+        controller = SoadsController(params['soads'], robot, verbosity)
+    elif 'tunnel_mpc' in params:
+        controller = TunnelMpcController(params['tunnel_mpc'], robot, verbosity)
+    elif 'tunnel_mpc_convergence' in params:
+        controller = TunnelMpcControllerConvergence(params['tunnel_mpc_convergence'], robot, verbosity)
+    else:
+        raise Exception("[Load Config]: No valid controller selection in param file.\n"
+                        "\t\t\tSelection: {}\n"
+                        "\t\t\tValid selections: [soads, tunnel_mpc, tunnel_mpc_convergence]".format(str(list(params.keys())[0])))
+
+    return scene, robot, controller, x0
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml
new file mode 100644
index 0000000..fe1dcbf
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/robot_params.yaml
@@ -0,0 +1,27 @@
+
+Omnidirectional:
+  model: 'Omnidirectional'
+  width: 0.5
+  vel_max: 1.5
+
+Unicycle_forward:
+  model: 'Unicycle'
+  width: 0.5
+  lin_vel_min: 0.
+  lin_vel_max: 1.5
+  ang_vel_max: 1.5
+
+Unicycle:
+  model: 'Unicycle'
+  width: 0.5
+  lin_vel_min: -0.5
+  lin_vel_max: 1.5
+  ang_vel_max: 1.5
+
+Bicycle:
+  model: 'Bicycle'
+  width: 0.5
+  steer_max: 1.0
+  lin_vel_min: -0.5
+  lin_vel_max: 1.5
+  steer_vel_max: 0.5
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml
new file mode 100644
index 0000000..8a902d0
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/soads_ctrl_params.yaml
@@ -0,0 +1,14 @@
+soads:
+  dt: 0.1
+  starify: 1
+  hull_epsilon: 0.5
+  max_compute_time: 1000
+  make_convex: 0
+  adapt_obstacle_velocity: 1
+  unit_magnitude: 0
+  crep: 1.5
+  reactivity: 2
+  tail_effect: 0
+  convergence_tolerance: 0.01
+  lin_vel: 1.
+  dp_decay_rate: 0.5
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml
new file mode 100644
index 0000000..6181186
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_convergence_params.yaml
@@ -0,0 +1,39 @@
+
+tunnel_mpc_convergence:
+  # Workspace modification
+  rho0: 0.3
+  gamma: 0.5
+  make_convex: 1
+  max_obs_compute_time: 40
+  hull_epsilon: 0.3
+  use_prev_workspace: 1
+  # Target generation
+  convergence_tolerance: 1.e-3
+  max_compute_time: 10
+  crep: 1.2
+  reactivity: 1.
+  buffer: 1
+  # MPC
+  ce: 100.
+  cs: 500.
+  e_penalty: 0
+  dg: 0.2
+  cg: 10000
+  R: [250, 2.5]
+  convergence_margin: 0.02
+  lambda: 0.2
+
+  #-------- MPC Build params --------- #
+  build_mode: 'release'
+  integration_method: 'euler'
+  n_pol: 10
+  N: 5
+  dt: 0.2
+  solver_tol: 1.e-5
+  solver_max_time: 500 # Maximum duration for mpc solver in milliseconds
+  solver_max_inner_iterations: 1000
+  solver_max_outer_iterations: 10
+  solver_initial_tol: 1.e-4
+  solver_delta_tol: 1.e-4
+  solver_weight_update_factor: 10.0
+  solver_initial_penalty: 1000.0
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml
new file mode 100644
index 0000000..ebc6ba8
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/params/tunnel_mpc_params.yaml
@@ -0,0 +1,38 @@
+
+tunnel_mpc:
+  # Workspace modification
+  rho0: 0.3
+  gamma: 0.5
+  make_convex: 1
+  max_obs_compute_time: 40
+  hull_epsilon: 0.3
+  use_prev_workspace: 1
+  # Target generation
+  convergence_tolerance: 1.e-3
+  max_compute_time: 10
+  crep: 1.
+  reactivity: 1.
+  buffer: 1
+  # MPC
+  ce: 100.
+  cs: 500.
+  e_penalty: 0
+  dg: 0.2
+  cg: 10000
+  R: [250, 2.5]
+  convergence_margin: 0.02
+
+  #-------- MPC Build params --------- #
+  build_mode: 'release'
+  integration_method: 'euler'
+  n_pol: 10
+  N: 5
+  dt: 0.2
+  solver_tol: 1.e-5
+  solver_max_time: 500 # Maximum duration for mpc solver in milliseconds
+  solver_max_inner_iterations: 1000
+  solver_max_outer_iterations: 10
+  solver_initial_tol: 1.e-4
+  solver_delta_tol: 1.e-4
+  solver_weight_update_factor: 10.0
+  solver_initial_penalty: 1000.0
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py
new file mode 100644
index 0000000..691e919
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/config/scene.py
@@ -0,0 +1,555 @@
+import numpy as np
+from starworlds.obstacles import Ellipse, StarshapedPolygon, motion_model, Polygon
+import matplotlib.pyplot as plt
+
+scene_description = {
+    1: "No obstacles.",
+    2: "1 static ellipse.",
+    3: "2 polygons. One static, one moving slowly towards, and into, the other.",
+    4: "Intersecting static ellipse and polygon. Ellipse moving towards polygon.",
+    5: "6 moving ellipses.",
+    6: "3 static polygons and 3 moving ellipses.",
+    7: "1 static polygon and 2 static ellipses.",
+    8: "3 static ellipses.",
+    9: "Corridor with moving ellipse into corridor.",
+    10: "Crowd.",
+    11: "Boundary.",
+    12: "Start stop in corridor. Ellipse obstacle."
+}
+
+
+class Scene:
+
+    def __init__(self, id=None, all_convex=False, scene_setup=None):
+        self.id = id
+        self.obstacles = None
+        self.p0 = None
+        self.pg = None
+        self.xlim = None
+        self.ylim = None
+        self._obstacles_to_plot = None
+
+        if id is None:
+            self.obstacles = scene_setup['obstacles']
+            self.p0 = scene_setup['p0']
+            self.pg = scene_setup['pg']
+            self.xlim = scene_setup['xlim']
+            self.ylim = scene_setup['ylim']
+            self._obstacles_to_plot = self.obstacles
+        elif id is not None:
+            self.init_scene_id(id, all_convex)
+        else:
+            print("Bad scene initialization")
+
+    def init_scene_id(self, id, all_convex):
+        scene_id = 0
+        # Scene 1
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+            ]
+            self.p0 = np.array([-5., 0.])
+            self.pg = np.array([5., 0.])
+            self.xlim = [-10, 10]
+            self.ylim = [-10, 10]
+
+        # Scene 2
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                Ellipse([1., 1.], motion_model=motion_model.Static(pos=[2., 0.])),
+            ]
+            self.p0 = np.array([0., -2.])
+            self.pg = np.array([2., 2.])
+            self.xlim = [-1, 5]
+            self.ylim = [-3, 3]
+
+        # Scene 3
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                # Polygon([[3, 0], [3, -4], [5, -4], [5, -2], [8, -2], [8, -4], [10, -4], [10, 0]]),
+                StarshapedPolygon([[3, 0], [3, -4], [5, -4], [5, -2], [8, -2], [8, 0]]),
+                StarshapedPolygon([[8, 0], [8, -4], [10, -4], [10, 0]]),
+                StarshapedPolygon([[6, -3], [6, -7], [7, -7], [7, -3]], motion_model=motion_model.SinusVelocity(pos=[0, -1], x2_mag=0.2))
+            ]
+            self.p0 = np.array([0, -4])
+            self.pg = np.array([11, -2])
+            self.xlim = [-1, 13]
+            self.ylim = [-8, 2]
+
+        # Scene 4
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                StarshapedPolygon([[4, 0], [8, 0], [8, 2], [6, 2], [6, 6], [4, 6]]),
+                Ellipse([1, 1], motion_model=motion_model.Static(pos=[4, -1])),
+                Ellipse([1, 1], motion_model=motion_model.Interval([8, -3], [(5, [8, -1])])),
+                # StarshapedPolygon([[1, -1], [1.5, -1], [1.5, 16], [1, 16]]),
+                # StarshapedPolygon([[2, -1], [3, -1], [3, 16], [2, 16]]),
+            ]
+            self.p0 = np.array([3., -4.])
+            # self.p0 = np.array([1.75, 0.5])
+            # self.theta0 = np.pi/2
+            self.pg = np.array([8., 6.])
+            self.xlim = [0, 10]
+            self.ylim = [-5, 8]
+
+        # Scene 5
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, -5], x1_mag=-0.2)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[4, -3], x1_mag=-0.5)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[7, -1], x1_mag=-0.2)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[5, -2], x1_mag=-0.25, x2_mag=-0.2)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[1, -5], x1_mag=0.3, x2_mag=0.5)),
+                Ellipse([0.4, 1], motion_model=motion_model.SinusVelocity(pos=[8, -3], x2_mag=2, x2_period=5)),
+                # Ellipse([2, 1], motion_model=motion_model.SinusVelocity(pos=[7, -2], x1_mag=2, x1_period=3)),
+                Ellipse([0.4,0.4], motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)),
+            ]
+            self.p0 = np.array([0., -3.])
+            self.pg = np.array([11., -3.])
+            self.xlim = [-1, 12]
+            self.ylim = [-8, 4]
+
+            # n_pol = 10
+            # t = np.linspace(0, 2 * np.pi, n_pol, endpoint=False)
+            # ell_polygon = np.vstack((1 * np.cos(t), 1 * np.sin(t))).T
+            # ell_polygon2 = np.vstack((0.4 * np.cos(t), 1 * np.sin(t))).T
+            # ell_polygon3 = np.vstack((0.4 * np.cos(t), 0.4 * np.sin(t))).T
+            # self.obstacles += [
+            #     StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[6, -5], x1_mag=-0.2)),
+            #     StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[4, -3], x1_mag=-0.5)),
+            #     StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[7, -1], x1_mag=-0.2)),
+            #     StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[5, -2], x1_mag=-0.25, x2_mag=-0.2)),
+            #     StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[1, -5], x1_mag=0.3, x2_mag=0.5)),
+            #     StarshapedPolygon(ell_polygon2, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[8, -3], x2_mag=2, x2_period=5)),
+            #     StarshapedPolygon(ell_polygon3, is_convex=True, motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)),
+            #     ]
+
+        # Scene 6
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                # Polygon([(8, 0), (9, 0), (9, 5), (7, 5), (7, 7), (6, 7), (6, 3), (8, 3)], is_convex=False),
+                StarshapedPolygon([(7, 3), (7, 7), (6, 7), (6, 3)], is_convex=True),
+                StarshapedPolygon([(11, 4), (7, 4), (7, 3), (11, 3)], is_convex=True),
+                StarshapedPolygon([(11, 1), (12, 1), (12, 5), (11, 5)], is_convex=True),
+                # StarshapedPolygon([(8, 5), (7, 5), (7, 3), (8, 3)], is_convex=True),
+                # StarshapedPolygon([(8, 0), (9, 0), (9, 5), (8, 5)], is_convex=True),
+                StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], is_convex=True),
+                # StarshapedPolygon([(6, 11), (2, 11), (2, 10), (6, 10)], is_convex=True),
+                # Ellipse([1, 1], motion_model=motion_model.Waypoints([12, 3], [(9.8, 5.5), (9.8, 9.5), (7, 9.5), (3, 4)], 1.)),
+                Ellipse([1, 1], motion_model=motion_model.Waypoints([3, 5.5], [(4, 5.5), (7.3, 9.5), (10, 9.5), (10.3, 4.5)], 1.)),
+                Ellipse([1, 1], motion_model=motion_model.Interval([5, 8], [(3, (7.6, 7.2))])),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([9, 10], [(5, (4.5, 10)), (5, (4.5, 4))]))
+                # Ellipse([1, 1], motion_model=motion_model.Interval([3, 4], [(5, (4., 5)), (5, (4., 9)), (4, (9, 9))]))
+                Ellipse([1, 1], motion_model=motion_model.Interval([3.3, 10], [(6, (3.3, 6)), (6, (5.5, 6))]))
+            ]
+            self.p0 = np.array([13, 6.])
+            self.pg = np.array([1., 5.])
+            # self.pg = np.array([1., 5.])
+            self.xlim = [0, 13]
+            self.ylim = [-1, 11]
+
+        # Scene 7
+        scene_id += 1
+        if id == scene_id:
+            ell_ax = [0.8, 0.8]
+            self.obstacles = [
+                StarshapedPolygon([(8, 5), (7, 5), (7, 3), (8, 3)], is_convex=True),
+                StarshapedPolygon([(7, 3), (7, 7), (6, 7), (6, 3)], is_convex=True),
+                StarshapedPolygon([(8, 0), (9, 0), (9, 5), (8, 5)], is_convex=True),
+                StarshapedPolygon([(3, 6), (4, 6), (4, 10), (3, 10)], is_convex=True),
+                StarshapedPolygon([(9.5, 6), (10.5, 6), (10.5, 10), (9.5, 10)], is_convex=True),
+                # Ellipse(ell_ax, motion_model=motion_model.Waypoints([12, 3], [(9.8, 5.5), (9.8, 9.5), (7, 9.5)], 0.8)),
+                Ellipse(ell_ax, motion_model=motion_model.Static([9, 9])),
+                Ellipse(ell_ax, motion_model=motion_model.Interval([12, 5], [(2, (11, 6))])),
+                Ellipse(ell_ax, motion_model=motion_model.Interval([5, 8], [(3, (7.8, 7))])),
+                # Ellipse(ell_ax, motion_model=motion_model.Interval([3, 4], [(5, (5, 4)), (5, (4.5, 9))])),
+                Ellipse(ell_ax, motion_model=motion_model.Interval([2, 3], [(3, (4., 6))])),
+                Ellipse(ell_ax, motion_model=motion_model.SinusVelocity([4.7, 12], x2_mag=-0.2)),
+            ]
+
+            # n_pol = 10
+            # t = np.linspace(0, 2 * np.pi, n_pol, endpoint=False)
+            # ell_polygon = np.vstack((1 * np.cos(t), 1 * np.sin(t))).T
+            # self.obstacles += [StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.Waypoints([12, 3], [(9.8, 5.5), (9.8, 9.5), (7, 9.5), (3, 4)], 1.))]
+            # self.obstacles += [StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.Interval([5, 8], [(3, (7.8, 7))]))]
+            # self.obstacles += [StarshapedPolygon(ell_polygon, is_convex=True, motion_model=motion_model.Interval([3, 4], [(5, (5, 4)), (5, (4.5, 9)), (4, (9, 9))]))]
+
+            self.p0 = np.array([12., 7.])
+            self.pg = np.array([1., 4.])
+            self.xlim = [0, 13]
+            self.ylim = [-1, 11]
+            # self.obstacles = [
+            #     StarshapedPolygon([(3, 7), (6, 7), (6, 5), (7, 5), (7, 8), (3, 8)]),
+            #     Ellipse([2, 0.5], motion_model=motion_model.Static([5, 4], -np.pi/3)),
+            #     Ellipse([2, 1], motion_model=motion_model.Static([9, 4], np.pi/2))
+            # ]
+            # self.p0 = np.array([1, 6])
+            # self.pg = np.array([11, 6])
+            # self.xlim = [0, 12]
+            # self.ylim = [0, 10]
+
+        # Scene 8
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                Ellipse([2, 1], motion_model=motion_model.Static([5, 6], -np.pi/3)),
+                Ellipse([1.2, 1.2], motion_model=motion_model.Static([4, 4], 0)),
+                Ellipse([1, 1], motion_model=motion_model.Static([4, 8], 0)),
+                Ellipse([1, 1], motion_model=motion_model.Static([9, 4], 0))
+            ]
+            self.p0 = np.array([1, 6])
+            self.pg = np.array([11, 6])
+            self.xlim = [0, 12]
+            self.ylim = [0, 10]
+
+        # Scene 9
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                # StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+                StarshapedPolygon([[2, 5], [8, 5], [8, 6], [2, 6]]),
+                StarshapedPolygon([[2, 2], [8, 2], [8, 3], [2, 3]]),
+                StarshapedPolygon([[2, 8], [8, 8], [8, 9], [2, 9]]),
+                Ellipse([1.1, 1.1], motion_model=motion_model.Interval([-2, 4], [(13, (10, 4))])),
+                # StarshapedPolygon(Ellipse([1, 1]).polygon(), motion_model=motion_model.Interval([-1, 4], [(9, (10, 4))])),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([-2, 4], [(9, (11, 4))])),
+            ]
+            # self.p0 = np.array([9, 5])
+            self.p0 = np.array([9, 4])
+            self.pg = np.array([0.5, 5.5])
+            self.xlim = [0, 10]
+            self.ylim = [2, 9]
+
+        # Scene 10
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                # StarshapedPolygon([[7, 13], [10, 10], [11, 11], [8, 14]]),
+                # StarshapedPolygon([[10, 11], [10, 7], [11, 7], [11, 11]]),
+                # StarshapedPolygon([[0, 0], [0, 1], [1, 1], [1, 0]], motion_model=motion_model.Interval([5, 11], [(5, [5, 6]), (2, [8, 6])])),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[12, 9], x1_mag=-0.2)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[15, 8], x1_mag=-0.8)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[10, 11], x1_mag=0.)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[10, 7], x1_mag=0.8)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[12, 11.5], x1_mag=0.2)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[15, 13.5], x2_mag=-0.1)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, 7], x1_mag=0.3)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[17, 5], x2_mag=0.1)),
+                Ellipse([1, 1], motion_model=motion_model.Interval([16, 4], [(11, [16, 7]), (5., [16, 10])])),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[3, 10], x1_mag=0.3)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[4, 8.5], x1_mag=0.1)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[5, 11.5], x1_mag=-0.1)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[8, 6], x1_mag=-0.1)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[10, 4], x2_mag=0.1)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[11, 3], x2_mag=0.2)),
+                Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[13, 13.5], x2_mag=-0.2)),
+                # Ellipse([1, 1], motion_model=motion_model.SinusVelocity(pos=[6, 7], x1_mag=0.3)),
+                # StarshapedPolygon([[0, 0], [0, 1], [1, 1], [1, 0]], motion_model=motion_model.Interval([14, 3], [(5, [9.5, 6.5])])),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([6, -5], [])),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([4, -3], [])),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([7, -1], [])),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([5, -2], x1_mag=-0.25, x2_mag=-0.2)),
+                # Ellipse([1, 1], motion_model=motion_model.Interval([1, -5], x1_mag=0.3, x2_mag=0.5)),
+                # Ellipse([0.4, 1], motion_model=motion_model.SinusVelocity(pos=[8, -3], x1_mag=3, x1_period=4, x2_mag=2,
+                #                                                           x2_period=2)),
+                # Ellipse([2, 1], motion_model=motion_model.SinusVelocity(pos=[7, -2], x1_mag=2, x1_period=3)),
+                # Ellipse([0.4, 0.4], motion_model=motion_model.SinusVelocity(pos=[8, -5], x1_mag=0.3, x2_mag=0.5)),
+            ]
+            self.p0 = np.array([1., 9.])
+            self.pg = np.array([19., 9.])
+            self.xlim = [0, 20]
+            self.ylim = [0, 18]
+
+        # Scene 11
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                # PolynomialObstacle(xys=np.array([[0, 0], [10, 0], [10, 3], [4, 5], [10, 7], [10, 10], [0, 10], [0, 0]], dtype=np.float64) + np.array([-2, -5]), pol_deg=60, is_boundary=True),
+                # Ellipse([5, 5], is_boundary=True),
+                Ellipse([1, 1], motion_model=motion_model.Interval([0., 0.5], [(0.5, [1.2, 0.5])])),
+                Ellipse([1, 1], motion_model=motion_model.Static([0, -0.5])),
+                Ellipse([1, 1], motion_model=motion_model.Static([0, 1.5]))
+            ]
+            self.p0 = np.array([-1.5, 1.5])
+            # self.p0 = np.array([5.6, -1.55])
+            self.pg = np.array([1.5, -1.5])
+            self.xlim = [-3, 3]
+            self.ylim = [-3, 3]
+
+        # Scene 12
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                StarshapedPolygon([[0, 0], [2, 0], [2, 6], [0, 6]]),
+                StarshapedPolygon([[2.5, 0], [6, 0], [6, 6], [2.5, 6]]),
+                StarshapedPolygon([[7, 0], [10, 0], [10, 6], [7, 6]]),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Interval([4.5, -0.3], [(3, (4.5, -0.3)), (1, (4.5, -2))]))
+                # StarshapedPolygon([[6.5, 0], [10, 0], [10, 6], [6.5, 6]]),
+            ]
+            self.p0 = np.array([2.25, 0.5])
+            self.pg = np.array([6.5, 1])
+            # self.pg = np.array([6.25, 1])
+            self.theta0 = np.pi/2
+            self.xlim = [0, 10]
+            self.ylim = [-4, 2]
+
+        # Scene 13
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 0.7])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([3.5, -0.1])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, -0.9])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([7, 0.6])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([8, -0.2])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([4.5, 1.8])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 0.4])),
+                              Ellipse([0.5, 0.5], motion_model=motion_model.Static([5., -1.4]))
+            ]
+            [o.polygon() for o in self.obstacles]
+
+
+            self.p0 = np.array([1, 0])
+            self.pg = np.array([9, 0])
+            # self.pg = np.array([6.25, 1])
+            # self.theta0 = np.pi / 2
+            self.xlim = [0, 10]
+            self.ylim = [-5, 5]
+
+        # Scene 14
+        scene_id += 1
+        if id == scene_id:
+            n_pol = 20
+            self.obstacles = [
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([3, 0.7], x1_mag=-0.13)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([4, -0.1], x1_mag=-0.5)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([3, -0.9], x1_mag=-0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([7, 0.6], x1_mag=-0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([8, -0.2], x1_mag=-0.3, x2_mag=-0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([6, -0.5], x1_mag=-0.1, x2_mag=0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([5.3, 0.4], x1_mag=-0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([5., -1.4], x2_mag=0.0)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([6.3, 1.2], x1_mag=0.)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([4.2, 1.4], x1_mag=-0.3, x2_mag=-0.2)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([8., 1.2], x1_mag=-0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([8.5, -0.75], x1_mag=-0.1)),
+                Ellipse([0.5, 0.5], n_pol=n_pol, motion_model=motion_model.SinusVelocity([7, -1.25], x1_mag=-0.05))
+            ]
+            # self.obstacles = [StarshapedPolygon(o.polygon(output_frame=Frame.OBSTACLE), motion_model=o._motion_model) for o in self.obstacles]
+
+            self.p0 = np.array([0.9, 0])
+            self.p0 = np.array([1.5, 0])
+            self.pg = np.array([9., 0])
+            # self.pg = np.array([6.25, 1])
+            # self.theta0 = np.pi / 2
+            self.xlim = [0, 10]
+            self.ylim = [-5, 5]
+
+        # Scene 15
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                Polygon([[3, 1], [4, 1], [4, 4], [2.5, 4], [2.5, 6], [1.5, 6], [1.5, 4], [0, 4], [0, 3], [3, 3]]),
+                Polygon([[4, 6], [5, 6], [5, 9], [2, 9], [2, 8], [2, 7.2], [3, 7.2], [3, 8], [4, 8]]),
+                StarshapedPolygon([[6, 6], [8, 6], [8, 7], [7, 7], [7, 8], [6, 8]]),
+                Polygon([[10, 8], [10, 2], [6, 2], [6, 4], [7, 4], [7, 3], [9, 3], [9, 8]]),
+                Ellipse([.4, .4], motion_model=motion_model.Static([5.5, 6.5])),
+                Ellipse([.4, .4], motion_model=motion_model.Waypoints([8.5, 6.5], [[8.5, 4.5], [4.5, 4.5]], 0.2)),
+                Ellipse([.4, .4], motion_model=motion_model.Waypoints([5, 4.5], [[5, 1]], 0.2)),
+                # Ellipse([.6, .6], motion_model=motion_model.Static([2, 6.5]))
+            ]
+            self.p0 = np.array([1, 1])
+            self.pg = np.array([5.5, 8])
+            self.xlim = [0, 10]
+            self.ylim = [0, 10]
+
+        # Scene 16
+        scene_id += 1
+        if id == scene_id:
+            # np.random.seed(2)
+            No = 30
+            ell_radius_mean = 0.5
+            ell_radius_std = 0.2
+            self.xlim = [0, 10]
+            self.ylim = [0, 10]
+
+            def random_scene_point():
+                return np.array([np.random.rand() * (self.xlim[1]-self.xlim[0]) + self.xlim[0],
+                                 np.random.rand() * (self.ylim[1]-self.ylim[0]) + self.ylim[0]])
+
+            self.obstacles = [
+                Ellipse(a=np.random.normal(ell_radius_mean, ell_radius_std, 2)) for _ in range(No)
+            ]
+            [o.set_motion_model(motion_model.Static(random_scene_point())) for o in self.obstacles]
+            self.p0 = random_scene_point()
+            while any([o.interior_point(self.p0) for o in self.obstacles]):
+                self.p0 = random_scene_point()
+            self.pg = random_scene_point()
+            while any([o.interior_point(self.pg) for o in self.obstacles]):
+                self.pg = random_scene_point()
+
+        # Scene 17
+        scene_id += 1
+        if id == scene_id:
+            self.obstacles = [
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 5.7])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([3.5, 4.9])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([3, 4.1])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([7, 5.6])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([8, 4.8])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([4.5, 6.8])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 5.4])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([5., 3.6])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 2.8])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([7., 6.2])),
+                Ellipse([0.5, 0.5], motion_model=motion_model.Static([5.3, 1.8]))
+            ]
+            self.p0 = np.array([1, 5])
+            self.pg = np.array([9, 5])
+            self.xlim = [0, 10]
+            self.ylim = [0, 10]
+
+        # Scene 18
+        scene_id += 1
+        if id == scene_id:
+            ell_ax = [0.8, 0.8]
+            self.obstacles = [
+                StarshapedPolygon([(7, 5), (6, 5), (6, 3), (7, 3)], is_convex=True),
+                StarshapedPolygon([(6, 3), (6, 7), (5, 7), (5, 3)], is_convex=True),
+                StarshapedPolygon([(7, 1), (8, 1), (8, 5), (7, 5)], is_convex=True),
+                StarshapedPolygon([(6, 7), (6, 8), (4, 8), (4, 7)], is_convex=True),
+                StarshapedPolygon([(2, 6), (3, 6), (3, 10), (2, 10)], is_convex=True),
+                StarshapedPolygon([(3, 9), (3, 10), (6, 10), (6, 9)], is_convex=True),
+                StarshapedPolygon([(8, 6), (9, 6), (9, 10), (8, 10)], is_convex=True),
+                Ellipse(ell_ax, motion_model=motion_model.Static([7.5, 8])),
+                Ellipse(ell_ax, motion_model=motion_model.Static([4.8, 5])),
+                # Ellipse(ell_ax, motion_model=motion_model.Static([4, 8])),
+                # Ellipse(ell_ax, motion_model=motion_model.Static([2.5, 5.5])),
+                Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])),
+                # Ellipse(ell_ax, motion_model=motion_model.Interval([2, 3], [(3, (4., 6))])),
+                # Ellipse(ell_ax, motion_model=motion_model.SinusVelocity([4.7, 12], x2_mag=-0.2)),
+            ]
+            self.p0 = np.array([1., 1.])
+            # self.p0 = np.array([1., 3.])
+            # self.p0 = np.array([1., 5.])
+            self.pg = np.array([9., 5.])
+            self.xlim = [-1, 11]
+            self.ylim = [-1, 11]
+            self.theta0 = 0
+
+        # Scene 19
+        scene_id += 1
+        if id == scene_id:
+            ell_ax = [0.8, 0.8]
+            self.obstacles = [
+                StarshapedPolygon([(4, 1), (6, 1), (6, 9), (4, 9)], is_convex=True),
+                StarshapedPolygon([(1, 4), (9, 4), (9, 6), (1, 6)], is_convex=True),
+                Ellipse(ell_ax, motion_model=motion_model.Static([6.3, 0.5])),
+                Ellipse(ell_ax, motion_model=motion_model.Static([1.2, 6.7])),
+                # Ellipse(ell_ax, motion_model=motion_model.Static([1.7, 8])),
+                Ellipse(ell_ax, motion_model=motion_model.Static([6, 0])),
+                Ellipse(ell_ax, motion_model=motion_model.Static([8.6, 6.7])),
+                # Ellipse(ell_ax, motion_model=motion_model.Static([2.5, 5.5])),
+                # Ellipse(ell_ax, motion_model=motion_model.Static([6.5, 1])),
+            ]
+            self.p0 = np.array([3, 7.])
+            self.p0 = np.array([7, 6.5])
+            # self.p0 = np.array([7., 3.])
+            self.pg = np.array([3., 3.])
+            self.xlim = [-2, 10]
+            self.ylim = [-2, 10]
+            self.theta0 = 0
+
+        # Scene 20
+        scene_id += 1
+        if id == scene_id:
+            ell_ax = [0.8, 0.8]
+            self.obstacles = [
+                # StarshapedPolygon([[0, 0], [4, 0], [4, 1], [1, 1], [1, 3], [0, 3]]),
+                StarshapedPolygon([[0, 0], [1, 0], [1, 3], [0, 3]]),
+                StarshapedPolygon([[1, 0], [3, 0], [3, 1], [1, 1]]),
+                StarshapedPolygon([[3, 0], [4, 0], [4, 3], [3, 3]]),
+                # Polygon([[0, 0], [4, 0], [4, 3], [3, 3], [3, 1], [1, 1], [1, 3], [0, 3]]),
+                Ellipse([1, 1], motion_model=motion_model.Static([-0.5, 3.7])),
+                Ellipse([0.5, 1.5], motion_model=motion_model.Static([4, -1.]))
+            ]
+            self.p0 = np.array([0, -2.])
+            self.pg = np.array([2, 2])
+            # self.pg = np.array([4, 4])
+            self.xlim = [-3, 6]
+            self.ylim = [-3, 6]
+
+        if not (0 < id < scene_id+1):
+                text = 'Invalid scene id: ' + str(id) + '\n\nScene scenarios\n---------\n'
+                for i, description in scene_description.items():
+                    text += str(i) + ": " + description + "\n"
+                raise Exception(text)
+
+        # Make all obstacles convex
+        self._obstacles_to_plot = self.obstacles
+        if all_convex:
+            convex_obstacles = []
+            for o in self.obstacles:
+                if o.is_convex():
+                    convex_obstacles += [o]
+                else:
+                    pol = py2dPol.from_tuples(np.asarray(o.polygon().exterior.coords)[:-1, :])
+                    conv_pols = py2dPol.convex_decompose(pol)
+                    for cp in conv_pols:
+                        convex_obstacles += [StarshapedPolygon([(v.x, v.y) for v in cp.points], is_convex=True)]
+            self.obstacles = convex_obstacles
+        # self._obstacles_to_plot = self.obstacles
+
+        # Compute all polygon
+        [o.polygon() for o in self.obstacles]
+        [o.is_convex() for o in self.obstacles]
+
+    def step(self, dt, X_exclude=None, dist=0):
+        [o.move(dt, X_exclude, dist) for o in self.obstacles]
+        [o.move(dt, X_exclude, dist) for o in self._obstacles_to_plot]
+
+    def init_plot(self, ax=None, obstacle_color='lightgrey', draw_p0=1, draw_pg=1, show_obs_name=False, draw_streamlines=0):
+        if ax is None:
+            _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+        line_handles = []
+        if draw_p0:
+            ax.plot(*self.p0, 'ks', markersize=10)
+        if draw_pg:
+            ax.plot(*self.pg, 'k*', markersize=10)
+
+        # lh = self.obstacles[0].init_plot(ax=ax, fc='None', ec='k', show_reference=False)
+        # line_handles.append(lh)
+        for o in self._obstacles_to_plot:
+            # lh = o.init_plot(ax=ax, fc=obstacle_color, ec='k', show_reference=True)
+            lh, _ = o.init_plot(ax=ax, fc=obstacle_color, ec='k', show_name=show_obs_name, show_reference=False)
+            line_handles.append(lh)
+        ax.set_xlim(self.xlim)
+        ax.set_ylim(self.ylim)
+
+        if draw_streamlines:
+            from motion_control.path_generator import plot_vector_field
+            plot_vector_field(self.pg, self.obstacles, ax, xlim=self.xlim, ylim=self.ylim, n=50)
+        # line_handles = scene.init_plot()
+        # scene.update_plot(line_handles)
+        # ax = plt.gca()
+        # plot_vector_field(scene.xg, scene.obstacles, ax, xlim=None, ylim=None, n=150)
+        # x = np.array([6.5, -2])
+        # ax.plot(*scene.obstacles[0].boundary_mapping(x), 'o')
+        # ax.quiver(*x, *f(x, scene.xg, scene.obstacles, unit_magnitude=1))
+        # plt.show()
+
+        return line_handles, ax
+
+    def update_plot(self, line_handles):
+        for i, o in enumerate(self._obstacles_to_plot):
+            o.update_plot(line_handles[i])
+
+    def draw(self, ax=None, obstacle_color='lightgrey', draw_p0=1, draw_pg=1, show_obs_name=False, draw_streamlines=0):
+        lh, ax = self.init_plot(ax, obstacle_color, draw_p0, draw_pg, show_obs_name, draw_streamlines)
+        self.update_plot(lh)
+        return lh, ax
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py
new file mode 100644
index 0000000..861796e
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/__init__.py
@@ -0,0 +1 @@
+from .soads import f, SoadsController, draw_vector_field
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py
new file mode 100644
index 0000000..8ea5bbe
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/soads/soads.py
@@ -0,0 +1,260 @@
+import numpy as np
+from starworlds.starshaped_hull import cluster_and_starify
+from starworlds.utils.misc import tic, toc
+
+class SoadsController:
+
+    def __init__(self, params, robot, verbose=False):
+        if not robot.__class__.__name__ == 'Omnidirectional':
+            raise NotImplementedError("SoadsController only implemented for Omnidirectional robots.")
+        self.params = params
+        self.robot = robot
+        self.obstacle_clusters = None
+        self.obstacles_star = []
+        self.dp_prev = None
+        self.verbose = verbose
+        self.timing = {'obstacle': 0, 'control': 0}
+
+    def compute_u(self, x, pg, obstacles):
+        p = self.robot.h(x)
+
+        if self.params['starify']:
+            self.obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles, p, pg,
+                                                                                        self.params['hull_epsilon'],
+                                                                                        self.params['max_compute_time'],
+                                                                                        previous_clusters=self.obstacle_clusters,
+                                                                                        # dx_prev=self.dp_prev,
+                                                                                        make_convex=self.params['make_convex'],
+                                                                                        verbose=self.verbose)
+            self.timing['obstacle'] = sum(obstacle_timing)
+            self.obstacles_star = [cl.cluster_obstacle for cl in self.obstacle_clusters]
+
+            # print(obstacle_timing)
+
+            # self.obstacles_star = []
+            # for cl in self.obstacle_clusters:
+            #     cl_obstacles = cl.obstacles
+            #     for i in range(len(cl_obstacles)):
+            #         cl_obstacles[i].set_xr(cl.cluster_obstacle.xr())
+            #     self.obstacles_star += cl_obstacles
+            # print(obstacle_timing, n_iter)
+            # print("(Cl/Adm/Hull) calculation ({:.2f}/{:.2f}/{:.2f}) [{:.0f}]".format(*obstacle_timing[:-1], n_iter))
+
+        else:
+            self.timing['obstacle'] += 0
+            self.obstacles_star = obstacles
+
+        # if any([o.interior_point(p) for o in self.obstacles_star]):
+        #     import matplotlib.pyplot as plt
+        #     from obstacles.tools import draw_shapely_polygon, is_cw, is_ccw, is_collinear, RayCone
+        #     from starshaped_hull import admissible_kernel
+        #     print("p in obstacles")
+        #     for cl in self.obstacle_clusters:
+        #         _, ax = plt.subplots()
+        #         # cl.cluster_obstacle.draw(ax=ax, fc='r', alpha=0.5)
+        #         for o in cl.obstacles:
+        #             o.draw(ax=ax, show_name=1)
+        #             # if o.id() == 1:
+        #             #     ad_ker = admissible_kernel(o, p)
+        #             #     # tp1, tp2 = o.tangent_points(p)
+        #             #     # if is_ccw(p, tp1, tp2):
+        #             #     #     print('ccw')
+        #             #     # elif is_cw(p, tp1, tp2):
+        #             #     #     print('cw')
+        #             #     # elif is_collinear(p, tp1, tp2):
+        #             #     #     print('collinear')
+        #             #     # ax.plot(*tp1, 'rx')
+        #             #     # ax.plot(*tp2, 'rs')
+        #             #     draw_shapely_polygon(ad_ker.polygon(), ax=ax, fc='y', alpha=0.3)
+        #
+        #         ax.plot(*p, 'o')
+        #         ax.set_xlim([0, 13])
+        #         ax.set_ylim([-1, 11])
+        #         adm_ker_robot = RayCone.intersection_same_apex([admissible_kernel(o, p) for o in cl.obstacles], debug_info={'o': obstacles, 'x': p, 'xlim': [0, 13], 'ylim': [-1, 11]})
+        #         if cl.admissible_kernel is not None:
+        #             # draw_shapely_polygon(cl.admissible_kernel, ax=ax, fc='y', alpha=0.3)
+        #             draw_shapely_polygon(adm_ker_robot.polygon(), ax=ax, fc='y', alpha=0.3)
+        #     plt.show()
+
+        t0 = tic()
+        dist_to_goal = np.linalg.norm(p - pg)
+        if self.params['lin_vel'] > 0:
+            lin_vel = min(self.params['lin_vel'], dist_to_goal / self.params['dt'])
+            unit_mag = True
+        else:
+            lin_vel = 1
+            unit_mag = False
+        dp = lin_vel * f(p, pg, self.obstacles_star, unit_magnitude=unit_mag, crep=self.params['crep'],
+                         reactivity=self.params['reactivity'], tail_effect=self.params['tail_effect'],
+                         adapt_obstacle_velocity=self.params['adapt_obstacle_velocity'],
+                         convergence_tolerance=self.params['convergence_tolerance'])
+
+        p_next = p + dp * self.params['dt']
+
+        p_next_in_obstacle = False
+        scale = 1.
+        while any([o.interior_point(p_next) for o in self.obstacles_star]):
+            dp *= self.params['dp_decay_rate']
+            p_next = p + dp * self.params['dt']
+            # Additional compute time check
+            if toc(t0) > self.params['max_compute_time']:
+                if self.verbose:
+                    print("[Max compute time in soads when adjusting for collision. ")
+                dp *= 0
+                break
+        self.timing['control'] = toc(t0)
+
+        self.dp_prev = dp
+
+        return dp
+
+
+# TODO: Check if can make more computationally efficient
+def f(r, rg, obstacles, adapt_obstacle_velocity=False, unit_magnitude=False, crep=1., reactivity=1., tail_effect=False,
+      convergence_tolerance=1e-4, d=False):
+    goal_vector = rg - r
+    goal_dist = np.linalg.norm(goal_vector)
+    if goal_dist < convergence_tolerance:
+        return 0 * r
+
+    No = len(obstacles)
+    fa = goal_vector / goal_dist  # Attractor dynamics
+    if No == 0:
+        return fa
+
+    ef = [-fa[1], fa[0]]
+    Rf = np.vstack((fa, ef)).T
+
+    mu = [obs.reference_direction(r) for obs in obstacles]
+    normal = [obs.normal(r) for obs in obstacles]
+    gamma = [obs.distance_function(r) for obs in obstacles]
+    # Compute weights
+    w = compute_weights(gamma, weightPow=1)
+
+    # Compute obstacle velocities
+    xd_o = np.zeros((2, No))
+    if adapt_obstacle_velocity:
+        for i, obs in enumerate(obstacles):
+            xd_o[:, i] = obs.vel_intertial_frame(r)
+
+    kappa = 0.
+    f_mag = 0.
+    for i in range(No):
+        # Compute basis matrix
+        E = np.zeros((2, 2))
+        E[:, 0] = mu[i]
+        E[:, 1] = [-normal[i][1], normal[i][0]]
+        # Compute eigenvalues
+        D = np.zeros((2, 2))
+        D[0, 0] = 1 - crep / (gamma[i] ** (1 / reactivity)) if tail_effect or normal[i].dot(fa) < 0. else 1
+        D[1, 1] = 1 + 1 / gamma[i] ** (1 / reactivity)
+        # Compute modulation
+        M = E.dot(D).dot(np.linalg.inv(E))
+        # f_i = M.dot(fa)
+        f_i = M.dot(fa - xd_o[:, i]) + xd_o[:, i]
+        # Compute contribution to velocity magnitude
+        f_i_abs = np.linalg.norm(f_i)
+        f_mag += w[i] * f_i_abs
+        # Compute contribution to velocity direction
+        nu_i = f_i / f_i_abs
+        nu_i_hat = Rf.T.dot(nu_i)
+        kappa_i = np.arccos(np.clip(nu_i_hat[0], -1, 1)) * np.sign(nu_i_hat[1])
+        kappa += w[i] * kappa_i
+    kappa_norm = abs(kappa)
+    f_o = Rf.dot([np.cos(kappa_norm), np.sin(kappa_norm) / kappa_norm * kappa]) if kappa_norm > 0. else fa
+
+    if unit_magnitude:
+        f_mag = 1.
+    return f_mag * f_o
+
+
+def compute_weights(
+    distMeas,
+    N=0,
+    distMeas_lowerLimit=1,
+    weightType="inverseGamma",
+    weightPow=2,
+):
+    """Compute weights based on a distance measure (with no upper limit)"""
+    distMeas = np.array(distMeas)
+    n_points = distMeas.shape[0]
+
+    critical_points = distMeas <= distMeas_lowerLimit
+
+    if np.sum(critical_points):  # at least one
+        if np.sum(critical_points) == 1:
+            w = critical_points * 1.0
+            return w
+        else:
+            # TODO: continuous weighting function
+            # warnings.warn("Implement continuity of weighting function.")
+            w = critical_points * 1.0 / np.sum(critical_points)
+            return w
+
+    distMeas = distMeas - distMeas_lowerLimit
+    w = (1 / distMeas) ** weightPow
+    if np.sum(w) == 0:
+        return w
+    w = w / np.sum(w)  # Normalization
+    return w
+
+
+def rollout(x, xg, obstacles, step_size=0.01, max_nr_steps=10000, max_distance=np.inf, convergence_threshold=0.05, crep=1., reactivity=1.):
+    xs = np.zeros((2, max_nr_steps))
+    xs[:, 0] = x
+    dist_travelled = 0
+    for k in range(1, max_nr_steps):
+        dist_to_goal = np.linalg.norm(xs[:, k-1]-xg)
+        if dist_to_goal < convergence_threshold or dist_travelled > max_distance:
+            return xs[:, :k]
+        else:
+            step_mag = min(step_size, dist_to_goal)
+            step_dir = f(xs[:, k-1], xg, obstacles, unit_magnitude=1, crep=crep, reactivity=reactivity)
+            xs[:, k] = xs[:, k-1] + step_mag * step_dir
+            while any([o.interior_point(xs[:, k]) for o in obstacles]):
+                step_mag /= 2.
+                xs[:, k] = xs[:, k-1] + step_mag * step_dir
+            dist_travelled += step_mag
+    return xs
+
+
+def draw_streamlines(xg, obstacles, ax, init_pos_list, step_size=0.01, max_nr_steps=10000, max_distance=np.inf,
+                     convergence_threshold=0.05, crep=1., reactivity=1.,
+                     arrow_step=0.5, color='k', linewidth=1, **kwargs):
+    for x0 in init_pos_list:
+        xs = rollout(x0, xg, obstacles, step_size, max_nr_steps, max_distance, convergence_threshold, crep, reactivity)
+        travelled_dist = np.cumsum(np.linalg.norm(np.diff(xs, axis=1), axis=0))
+        travelled_dist = np.insert(travelled_dist, 0, 0)
+        arrow_dist = np.arange(0, travelled_dist[-1], arrow_step)
+        arrows_X = np.interp(arrow_dist, travelled_dist, xs[0, :])
+        arrows_Y = np.interp(arrow_dist, travelled_dist, xs[1, :])
+        arrows_U = np.zeros_like(arrows_X)
+        arrows_V = np.zeros_like(arrows_X)
+        # ax.plot(arrows_X, arrows_Y, 'k*')
+        for j in range(len(arrow_dist)):
+            arrows_U[j], arrows_V[j] = f(np.array([arrows_X[j], arrows_Y[j]]), xg, obstacles, unit_magnitude=1,
+                                         crep=crep, reactivity=reactivity)
+        ax.quiver(arrows_X, arrows_Y, arrows_U, arrows_V, color=color, scale=50, width=0.005, scale_units='width', headlength=2, headaxislength=2)
+        # ax.plot(xs[0, :], xs[1, :], color=color, linewidth=linewidth, **kwargs)
+
+def draw_vector_field(xg, obstacles, ax, xlim=None, ylim=None, n=50, **kwargs):
+    if xlim is None:
+        xlim = ax.get_xlim()
+    if ylim is None:
+        ylim = ax.get_ylim()
+
+    Y, X = np.mgrid[ylim[0]:ylim[1]:n*1j, xlim[0]:xlim[1]:n*1j]
+    U = np.zeros(X.shape)
+    V = np.zeros(X.shape)
+    mask = np.zeros(U.shape, dtype=bool)
+    for i in range(X.shape[1]):
+        for j in range(X.shape[0]):
+            x = np.array([X[i, j], Y[i, j]])
+            if np.any([o.interior_point(x) for o in obstacles]):
+                mask[i, j] = True
+                U[i, j] = np.nan
+            else:
+                U[i, j], V[i, j] = f(x, xg, obstacles, unit_magnitude=1)
+    U = np.ma.array(U, mask=mask)
+    # return ax.quiver(X, Y, U, V)
+    return ax.streamplot(X, Y, U, V, **kwargs)
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py
new file mode 100644
index 0000000..e6a308c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/__init__.py
@@ -0,0 +1,4 @@
+from .workspace_modification import workspace_modification
+from .path_generator import path_generator, pol2pos
+from .tunnel_mpc import TunnelMpc
+from .tunnel_mpc_controller import TunnelMpcController
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/mpc_build/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py
new file mode 100644
index 0000000..fae769c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/path_generator.py
@@ -0,0 +1,108 @@
+import numpy as np
+import time
+import shapely
+from starworlds.utils.misc import tic, toc
+from motion_control.soads.soads import f as soads_f
+
+
+def pol2pos(path_pol, s):
+    n_pol = len(path_pol) // 2 - 1
+    return [sum([path_pol[j * (n_pol + 1) + i] * s ** (n_pol - i) for i in range(n_pol + 1)]) for j in range(2)]
+
+
+def path_generator(r0, rg, obstacles, dp_max, N, dt, max_compute_time, n_pol, ds_decay_rate=0.5,
+                   ds_increase_rate=2., max_nr_steps=1000, convergence_tolerance=1e-5, P_prev=None, s_prev=None,
+                   reactivity=1., crep=1., tail_effect=False, reference_step_size=0.5, verbosity=0):
+    t0 = tic()
+
+    # Initialize
+    ds = 1
+    s = np.zeros(max_nr_steps)
+    r = np.zeros((max_nr_steps, r0.size))
+    if P_prev is not None:
+        i = P_prev.shape[0]
+        r[:i, :] = P_prev
+        s[:i] = s_prev
+    else:
+        i = 1
+        r[0, :] = r0
+
+    while True:
+        dist_to_goal = np.linalg.norm(r[i - 1, :] - rg)
+        # Check exit conditions
+        if dist_to_goal < convergence_tolerance:
+            if verbosity > 1:
+                print("[Path Generator]: Path converged. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+        if s[i - 1] >= N:
+            if verbosity > 1:
+                print("[Path Generator]: Completed path length. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+        if toc(t0) > max_compute_time:
+            if verbosity > 1:
+                print("[Path Generator]: Max compute time in path integrator. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+        if i >= max_nr_steps:
+            if verbosity > 1:
+                print("[Path Generator]: Max steps taken in path integrator. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+
+        # Movement using SOADS dynamics
+        dr = min(dp_max, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, adapt_obstacle_velocity=False,
+                                                 unit_magnitude=True, crep=crep,
+                                                 reactivity=reactivity, tail_effect=tail_effect,
+                                                 convergence_tolerance=convergence_tolerance)
+
+        r[i, :] = r[i - 1, :] + dr * ds
+
+        ri_in_obstacle = False
+        while any([o.interior_point(r[i, :]) for o in obstacles]):
+            if verbosity > 1:
+                print("[Path Generator]: Path inside obstacle. Reducing integration step from {:5f} to {:5f}.".format(ds, ds*ds_decay_rate))
+            ds *= ds_decay_rate
+            r[i, :] = r[i - 1, :] + dr * ds
+            # Additional compute time check
+            if toc(t0) > max_compute_time:
+                ri_in_obstacle = True
+                break
+        if ri_in_obstacle:
+            continue
+
+        # Update travelled distance
+        s[i] = s[i - 1] + ds
+        # Try to increase step rate again
+        ds = min(ds_increase_rate * ds, 1)
+        # Increase iteration counter
+        i += 1
+
+    r = r[:i, :]
+    s = s[:i]
+
+    # Evenly spaced path
+    s_vec = np.arange(0, s[-1] + reference_step_size, reference_step_size)
+    xs, ys = np.interp(s_vec, s, r[:, 0]), np.interp(s_vec, s, r[:, 1])
+    # Append not finished path with fixed final position
+    s_vec = np.append(s_vec, np.arange(s[-1] + reference_step_size, N + reference_step_size, reference_step_size))
+    xs = np.append(xs, xs[-1] * np.ones(len(s_vec)-len(xs)))
+    ys = np.append(ys, ys[-1] * np.ones(len(s_vec)-len(ys)))
+
+    reference_path = [el for p in zip(xs, ys) for el in p]
+
+    # TODO: Fix when close to goal
+    # TODO: Adjust for short arc length, skip higher order terms..
+    path_pol = np.polyfit(s_vec, reference_path[::2], n_pol).tolist() + \
+               np.polyfit(s_vec, reference_path[1::2], n_pol).tolist()
+    # Force init position to be correct
+    path_pol[n_pol] = reference_path[0]
+    path_pol[-1] = reference_path[1]
+
+    # Compute polyfit approximation error
+    epsilon = [np.linalg.norm(np.array(reference_path[2 * i:2 * (i + 1)]) - np.array(pol2pos(path_pol, s_vec[i]))) for i in
+               range(N + 1)]
+
+    compute_time = toc(t0)
+    return path_pol, epsilon, reference_path, compute_time
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py
new file mode 100644
index 0000000..f72b24f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc.py
@@ -0,0 +1,377 @@
+import casadi.casadi as casadi
+import opengen as og
+import numpy as np
+import os, sys
+import yaml
+
+
+class NoSolutionError(Exception):
+    '''raise this when there's no solution to the mpc problem'''
+    pass
+
+
+class TunnelMpc:
+
+    def __init__(self, params, robot):
+        self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build')
+        # Load parameters
+        self.build_params = None
+        self.robot = robot
+        self.set_build_params(params)
+        rebuild, self.build_name = self.get_build_version()
+        # Build if different from latest build
+        if rebuild:
+            self.build()
+        else:
+            print("Found MPC build: {}".format(self.build_name))
+
+        sys.path.insert(1, os.path.join(self.build_dir, self.build_name))
+        optimizer = __import__(self.build_name)
+        self.solver = optimizer.solver()
+        self.reset()
+
+    def reset(self):
+        self.sol_prev = None
+
+    def set_build_params(self, params):
+        self.build_params = {
+            'mpc_ver': 5,
+            'mode': params['build_mode'],
+            'N': params['N'],
+            'dt': params['dt'],
+            'solver_tol': params['solver_tol'],
+            'solver_max_time': params['solver_max_time'],
+            'solver_max_inner_iterations': params['solver_max_inner_iterations'],
+            'solver_max_outer_iterations': params['solver_max_outer_iterations'],
+            'x_min': self.robot.x_min,
+            'x_max': self.robot.x_max,
+            'u_min': self.robot.u_min,
+            'u_max': self.robot.u_max,
+            'nx': self.robot.nx,
+            'nu': self.robot.nu,
+            'np': 2,
+            'robot_model': self.robot.__class__.__name__,
+            'n_pol': params['n_pol'],
+            'integration_method': params['integration_method']
+        }
+
+    def get_build_version(self):
+        builds = [name for name in os.listdir(self.build_dir)
+                  if os.path.isdir(os.path.join(self.build_dir, name))]
+        for build_ver in builds:
+            if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')):
+                continue
+            with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file:
+                build_ver_params = yaml.load(file, Loader=yaml.FullLoader)
+            if self.build_params == build_ver_params:
+                return False, build_ver
+        ver = 0
+        while 'ver' + str(ver) in os.listdir(self.build_dir):
+            ver += 1
+        return True, 'ver' + str(ver)
+
+
+    def e_cost(self, e, ce):
+        return ce * e
+
+    def ud_cost(self, u, u_prev, R):
+        ud = [(u[i] - u_prev[i]) for i in range(self.robot.nu)]
+        return R[0] * ud[0] ** 2 + R[1] * ud[1] ** 2
+
+    def ref(self, path_pol, s):
+        return [sum([path_pol[j*(self.build_params['n_pol']+1) + i] * s**(self.build_params['n_pol']-i) for i in range(self.build_params['n_pol']+1)])
+                for j in range(self.build_params['np'])]
+
+    def fz(self, z, mu):
+        return self.robot.f(z[:-1], mu[:-1]) + [mu[-1] / self.build_params['dt']]
+
+    def fz_d(self, z, mu):
+        nz = self.build_params['nx'] + 1
+        if self.build_params['integration_method'] == 'euler':
+            zd = self.fz(z, mu)
+            return [z[i] + zd[i] * self.build_params['dt'] for i in range(nz)]
+        # if self.build_params['integration_method'] == 'RK4':
+        #     k1 = self.fz(z, u, v, path_pol)
+        #     k2 = self.fz([z[i] + self.build_params['dt'] / 2 * k1[i] for i in range(nz)], u, v, path_pol)
+        #     k3 = self.fz([z[i] + self.build_params['dt'] / 2 * k2[i] for i in range(nz)], u, v, path_pol)
+        #     k4 = self.fz([z[i] + self.build_params['dt'] * k3[i] for i in range(nz)], u, v, path_pol)
+        #     return [z[i] + (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) * self.build_params['dt'] / 6 for i in range(nz)]
+        raise NotImplemented
+
+    def one_step_feasible(self, sol, x0, path_pol, e_max, d=False):
+        u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol)
+        sol_ok = True
+        eps = 1e-6
+        # x constraints
+        if not all([self.build_params['x_min'][i] - eps <= x[0][i] <= self.build_params['x_max'][i] + eps for i in
+                    range(self.build_params['nx'])]):
+            sol_ok = False
+            if d:
+                print("Bad x")
+                print(x[:self.build_params['nx']])
+        # s constraints
+        if not (0 - eps <= s[1] <= self.build_params['N'] + eps):
+            sol_ok = False
+            if d:
+                print("Bad s")
+                print(s[1])
+        # u constraints
+        if not all([self.build_params['u_min'][i] - eps <= u[i] <= self.build_params['u_max'][i] + eps for i in
+                    range(self.build_params['nu'])]):
+            sol_ok = False
+            if d:
+                print("Bad u")
+                print(u[:self.build_params['nu']])
+        # v constraints
+        if not (0 - eps <= ds[0] <= np.inf + eps):
+            sol_ok = False
+            if d:
+                print("Bad ds")
+                print(ds[0])
+        # e constraints
+        if not (e[1] <= e_max + eps):
+            sol_ok = False
+            if d:
+                print("Bad e ({:.4f} > {:.4f})".format(e[1], e_max))
+
+        # All constraints ok
+        return sol_ok
+
+    def is_feasible(self, sol, x0, path_pol, e_max, d=False):
+        u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol)
+        sol_ok = True
+
+        eps = 1e-2
+        # x constraints
+        if not all([self.build_params['x_min'][i]-eps <= xk[i] <= self.build_params['x_max'][i]+eps for i in range(self.build_params['nx']) for xk in x]):
+            sol_ok = False
+            if d:
+                print("Bad x")
+                print(x)
+        # s constraints
+        if not all([0-eps <= sk <= self.build_params['N'] + eps for sk in s]):
+            sol_ok = False
+            if d:
+                print("Bad s")
+                print(s)
+        # u constraints
+        if not all([self.build_params['u_min'][i]-eps <= uk <= self.build_params['u_max'][i]+eps for i in range(self.build_params['nu']) for uk in u[i::self.build_params['nu']]]):
+            sol_ok = False
+            if d:
+                print("Bad u")
+                print(u)
+        # v constraints
+        if not all([0-eps <= dsk <= np.inf * self.build_params['dt'] + eps for dsk in ds]):
+            sol_ok = False
+            if d:
+                print("Bad ds")
+                print(ds)
+        # e constraints
+        if not all([ek <= e_max + eps for ek in e]):
+            sol_ok = False
+            if d:
+                print("Bad e")
+                print(e)
+
+        # All constraints ok
+        return sol_ok
+
+    def e_sqr(self, z, path_pol):
+        ref = self.ref(path_pol, z[-1])
+        p = self.robot.h(z[:-1])
+        e_sqr = sum([(ref[i] - p[i])**2 for i in range(self.build_params['np'])])
+        return e_sqr
+
+    def unpack_solution(self, sol, x0, path_pol):
+        u = sol[:self.build_params['nu'] * self.build_params['N']]
+        ds = sol[self.build_params['nu'] * self.build_params['N']:]
+        zk = x0 + [0]
+        x = [zk[:-1]]
+        s = [zk[-1]]
+        e = [self.e_sqr(zk, path_pol)]
+        for k in range(self.build_params['N']):  # LOOP OVER TIME STEPS
+            # Dynamics
+            muk = u[self.build_params['nu'] * k:self.build_params['nu'] * (k + 1)] + [ds[k]]
+            zk = self.fz_d(zk, muk)
+            x += [zk[:-1]]
+            s += [zk[-1]]
+            e += [np.sqrt(self.e_sqr(zk, path_pol))]
+        return u, ds, x, s, e
+
+    def sol2cost(self, sol, x0, path_pol, u_prev, params):
+        u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol)
+        u = u_prev + u
+        v_cost, ud_cost, omega_cost = [0.] * 3
+        for i in range(len(ds)):
+            ui_prev = u[self.build_params['nu'] * i:self.build_params['nu'] * (i + 1)]
+            ui = u[self.build_params['nu'] * (i+1):self.build_params['nu'] * (i + 2)]
+            ud_cost += self.ud_cost(ui, ui_prev, params['R'])
+            v_cost += self.v_cost(ds[i], params['cv'], params['lambda'], i)
+        e_end_cost = self.e_cost(e[-1]**2, params['ce_end'])
+        return v_cost, ud_cost, e_end_cost, omega_cost
+
+    def build(self):
+        # Build parametric optimizer
+        # ------------------------------------
+        params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'], 'path_pol': self.build_params['np'] * (self.build_params['n_pol']+1),
+                  'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'e_max': 1, 'rg': self.build_params['np'], 'e_penalty': 1, 'cg': 1}
+        par_dim = sum(list(params.values()))
+
+
+        # Exchange parameter dimension with value
+        par = casadi.SX.sym("par", par_dim)  # Parameters
+        p_idx = 0
+        for key, dim in params.items():
+            params[key] = par[p_idx:p_idx + dim]
+            p_idx += dim
+
+        # Init value
+        uk_prev = params['u_prev']
+        zk = casadi.vertcat(params['x0'], 0)
+        cost = 0
+        # Decision variable
+        mu = casadi.SX.sym('uds', (self.build_params['nu'] + 1) * self.build_params['N'])
+        # Constraint variables
+        f1, f1_min, f1_max = [], [], []
+        if True:
+            for k in range(self.build_params['N']):  # LOOP OVER TIME STEPS
+                # Decision variables
+                uk = mu[k * self.build_params['nu']:(k + 1) * self.build_params['nu']]
+                dsk = mu[self.build_params['nu'] * self.build_params['N'] + k]
+                muk = casadi.vertcat(uk, dsk)
+
+                # Step forward
+                zk1 = casadi.vertcat(*self.fz_d(zk, muk))
+                ek1_sqr = self.e_sqr(zk1, params['path_pol'])
+
+                # Add stage cost
+                cost += params['e_penalty'] * ek1_sqr
+                cost += self.ud_cost(uk, uk_prev, params['R'])
+
+                # Add state and error constraint
+                f1 = casadi.vertcat(f1, zk1, ek1_sqr - params['e_max']**2)
+                f1_min += self.build_params['x_min'] + [0] + [-1e10]
+                f1_max += self.build_params['x_max'] + [self.build_params['N']] + [0]
+
+                uk_prev = uk
+                zk = zk1
+
+
+            ## Terminal cost
+            # Cost on last tracking error
+            cost += self.e_cost(ek1_sqr, params['ce'])
+            cost += - params['cs'] * zk1[-1]
+            # Convergence cost
+            p_end = self.robot.h(zk[:-1])
+            xg_err = sum([(p_end[i] - params['rg'][i])**2 for i in range(self.build_params['np'])])
+            cost += params['cg'] * xg_err
+            # Final orientation cost
+            # cost += params['cg_orientation'] * zk[-1] / (self.build_params['N'] * self.build_params['dt']) * (zk[2]-params['rg'][2])**2
+
+        # Constraint set
+        set_c = og.constraints.Rectangle(f1_min, f1_max)
+        # Decision variable constraints
+        mu_bounds = og.constraints.Rectangle(self.build_params['u_min'] * self.build_params['N'] +
+                                               [0.] * self.build_params['N'],
+                                               self.build_params['u_max'] * self.build_params['N'] +
+                                               [1] * self.build_params['N'])
+
+        # Setup builder
+        problem = og.builder.Problem(mu, par, cost) \
+            .with_constraints(mu_bounds) \
+            .with_aug_lagrangian_constraints(f1, set_c)
+        build_config = og.config.BuildConfiguration() \
+            .with_build_directory(self.build_dir) \
+            .with_build_mode(self.build_params['mode']) \
+            .with_build_python_bindings()
+        meta = og.config.OptimizerMeta() \
+            .with_optimizer_name(self.build_name)
+        solver_config = og.config.SolverConfiguration() \
+            .with_tolerance(self.build_params['solver_tol']) \
+            .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \
+            .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \
+            .with_max_outer_iterations(self.build_params['solver_max_outer_iterations'])
+        builder = og.builder.OpEnOptimizerBuilder(problem,
+                                                  meta,
+                                                  build_config,
+                                                  solver_config) \
+            .with_verbosity_level(1)
+        builder.build()
+        print('')
+
+        # Save build params
+        with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file:
+            yaml.dump(self.build_params, file)
+
+    def trivial_solution(self, x, path_pol, e_max):
+        s_kappa = 0.9 * e_max / self.robot.vmax
+        p_ref = self.ref(path_pol, s_kappa)
+
+        u = [0] * (self.build_params['N'] * self.build_params['nu'])
+        ds = [0] * (self.build_params['N'])
+        xk = x
+        sk = 0
+        zk = xk + [sk]
+        for k in range(self.build_params['N']):  # LOOP OVER TIME STEPS
+            # Control law
+            if self.build_params['robot_model'] == 'Unicycle':
+                ref_dist = np.linalg.norm(np.array(xk[:2])-np.array(p_ref))
+                if ref_dist > 1e-5:
+                    theta_ref = np.arctan2(p_ref[1]-xk[1], p_ref[0]-xk[0])
+                    theta_diff = theta_ref - xk[-1]
+                    if theta_diff > np.pi:
+                        theta_diff -= 2 * np.pi
+                    if theta_diff < -np.pi:
+                        theta_diff += 2 * np.pi
+                    if abs(theta_diff) < 1e-2: # Less than 0.57 degree error
+                        # Linear velocity
+                        u[k*self.build_params['nu']] = min(self.build_params['u_max'][0], ref_dist / self.build_params['dt'])
+                    else:
+                        # Angular velocity
+                        if theta_diff > 0:
+                            u[k * self.build_params['nu'] + 1] = min(self.build_params['u_max'][1], theta_diff / self.build_params['dt'])
+                        else:
+                            u[k * self.build_params['nu'] + 1] = max(self.build_params['u_min'][1], theta_diff / self.build_params['dt'])
+
+            # Dynamics
+            muk = u[k * self.build_params['nu']:k * self.build_params['nu'] + 2] + [0]
+            zk = self.fz_d(zk, muk)
+            xk = zk[:-1]
+
+        return u + ds
+
+    def run(self, x, u_prev, path_pol, params, e_max, rg, verbosity=0):
+        robot_within_dg = np.linalg.norm(np.array(self.robot.h(rg))-np.array(self.robot.h(x))) < params['dg']
+        cg = params['cg'] if robot_within_dg else 0
+
+        p = x + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + \
+            [0.95*e_max] + rg + [params['e_penalty'], cg]
+
+        if self.sol_prev is None or not self.is_feasible(self.sol_prev, x, path_pol, 2*e_max):
+            # Use trivial solution as initial guess
+            self.sol_prev = self.trivial_solution(x, path_pol, e_max)
+        # Run solver
+        solution_data = self.solver.run(p=p, initial_guess=self.sol_prev)
+
+
+        # TODO: When not one step feasible, check for actual collision with mpc before using trivial solution.
+        if solution_data is None or not self.one_step_feasible(solution_data.solution, x, path_pol, e_max, d=verbosity>0):
+            if verbosity > 0:
+                print("[MPC]: Not feasible solution. Using trivial solution.")
+            sol = self.trivial_solution(x, path_pol, e_max)
+            self.sol_prev = None
+            exit_status = "Trivial solution"
+            cost = -1
+        else:
+            sol = solution_data.solution
+            exit_status = solution_data.exit_status
+            cost = solution_data.cost
+
+            self.sol_prev = sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \
+                            sol[self.build_params['nu'] * (self.build_params['N']-1):self.build_params['nu'] * self.build_params['N']] \
+                            + sol[self.build_params['nu'] * self.build_params['N']:]
+
+        # Unpack solution
+        u, ds, x_pred, s_pred, e_pred = self.unpack_solution(sol, x, path_pol)
+
+        return {'u': u, 'x': x_pred, 'e': e_pred, 's': s_pred, 'cost': cost, 'exit_status': exit_status}
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py
new file mode 100644
index 0000000..fa846a8
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/tunnel_mpc_controller.py
@@ -0,0 +1,101 @@
+from motion_control.tunnel_mpc import workspace_modification, path_generator, pol2pos, TunnelMpc
+from starworlds.utils.misc import tic, toc
+import numpy as np
+
+
+class TunnelMpcController:
+    def __init__(self, params, robot, verbosity=0):
+        self.params = params
+        self.params['dp_max'] = robot.vmax * self.params['dt']
+        self.robot = robot
+        self.mpc = TunnelMpc(params, robot)
+        self.verbosity = verbosity
+        self.reset()
+
+    def reset(self):
+        self.mpc.reset()
+        self.obstacle_clusters = None
+        self.obstacles_star = []
+        self.free_star = None
+        self.r0 = None
+        self.rg = None
+        self.target_path = []
+        self.path_pol = None
+        self.rho = None
+        self.epsilon = None
+        self.solution = None
+        self.u_prev = [0] * self.robot.nu
+        self.timing = {'workspace': 0, 'target': 0, 'mpc': 0}
+
+    def compute_u(self, x, pg, obstacles):
+        p = self.robot.h(x)
+
+        # Update obstacles
+        if not self.params['use_prev_workspace']:
+            self.free_star = None
+        self.obstacle_clusters, self.r0, self.rg, self.rho, self.free_star, self.timing['workspace'], exit_flag = \
+            workspace_modification(obstacles, p, pg, self.params['rho0'], self.params['max_obs_compute_time'],
+                                   self.params['hull_epsilon'], self.params['gamma'],
+                                   make_convex=self.params['make_convex'], previous_obstacle_clusters=self.obstacle_clusters,
+                                   free_star_prev=self.free_star, verbosity=self.verbosity)
+        self.obstacles_star = [o.cluster_obstacle for o in self.obstacle_clusters]
+
+        # Buffer previous target path
+        P_prev, s_prev = None, None
+        if self.params['buffer'] and self.target_path:
+            P_prev = np.array([self.target_path[::2], self.target_path[1::2]]).T
+            # Shift path to start at point closest to robot position
+            P_prev = P_prev[np.argmin(np.linalg.norm(p - P_prev, axis=1)):, :]
+            # P_prev[0, :] = self.r0
+            if np.linalg.norm(p - P_prev[0, :]) > self.rho:
+                if self.verbosity > 0:
+                    print("[Path Generator]: No reuse of previous path. Path not within distance rho from robot.")
+                P_prev = None
+            else:
+                for r in P_prev:
+                    if any([o.interior_point(r) for o in self.obstacles_star]):
+                        if self.verbosity > 0:
+                            print("[Path Generator]: No reuse of previous path. Path not collision-free.")
+                        P_prev = None
+
+            #     P_prev = None
+            if P_prev is not None:
+                # Cut off stand still padding in previous path
+                P_prev_stepsize = np.linalg.norm(np.diff(P_prev, axis=0), axis=1)
+                s_prev = np.hstack((0, np.cumsum(P_prev_stepsize) / self.params['dp_max']))
+                P_prev_mask = [True] + (P_prev_stepsize > 1e-8).tolist()
+                P_prev = P_prev[P_prev_mask, :]
+                s_prev = s_prev[P_prev_mask]
+
+        # Make sure all polygon representations are computed
+        [o._compute_polygon_representation() for o in self.obstacles_star]
+
+        # Check for convergence
+        if np.linalg.norm(np.array(p) - np.array(pg)) < self.params['convergence_margin']:
+            self.timing['target'] = 0
+            self.timing['mpc'] = 0
+            return np.zeros(self.robot.nu)
+
+        # Update target path
+        self.path_pol, self.epsilon, self.target_path, self.timing['target'] = \
+            path_generator(self.r0, self.rg, self.obstacles_star, self.params['dp_max'], self.params['N'],
+                           self.params['dt'], self.params['max_compute_time'], self.params['n_pol'],
+                           ds_decay_rate=0.5, ds_increase_rate=2., max_nr_steps=1000, P_prev=P_prev, s_prev=s_prev,
+                           reactivity=self.params['reactivity'], crep=self.params['crep'],
+                           convergence_tolerance=self.params['convergence_tolerance'], verbosity=self.verbosity)
+
+        # Compute MPC solution
+        t0 = tic()
+        # Set parameter for tracking error constraint
+        if self.rho > 0:
+            e_max = self.rho - max(self.epsilon)
+        else:
+            e_max = 1.e6
+
+        self.solution = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, e_max, self.rg.tolist(), verbosity=self.verbosity)
+        self.u_prev = self.solution['u'][:self.robot.nu]
+        self.timing['mpc'] = toc(t0)
+
+        # Extract first control signal
+        return np.array(self.solution['u'][:self.mpc.build_params['nu']])
+
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py
new file mode 100644
index 0000000..86d0f3d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc/workspace_modification.py
@@ -0,0 +1,66 @@
+import numpy as np
+from starworlds.starshaped_hull import cluster_and_starify, ObstacleCluster
+from starworlds.utils.misc import tic, toc
+import shapely
+
+# TODO: Check why computational time varies so much for same static scene
+def workspace_modification(obstacles, p, pg, rho0, max_compute_time, hull_epsilon, gamma=0.5, make_convex=0,
+                            previous_obstacle_clusters=None, free_star_prev=None, verbosity=0):
+
+    # Clearance variable initialization
+    rho = rho0 / gamma  # First rho should be rho0
+    t_init = tic()
+
+    while True:
+        if toc(t_init) > max_compute_time:
+            if verbosity > 0:
+                print("[Workspace modification]: Max compute time in rho iteration.")
+            break
+
+        # Reduce rho
+        rho *= gamma
+
+        # Pad obstacles with rho
+        obstacles_rho = [o.dilated_obstacle(padding=rho, id="duplicate") for o in obstacles]
+
+        # TODO: Fix boundaries
+        free_rho = shapely.geometry.box(-20, -20, 20, 20)
+        for o in obstacles_rho:
+            free_rho = free_rho.difference(o.polygon())
+
+        # TODO: Check buffering fix
+        # Find P0
+        Bp = shapely.geometry.Point(p).buffer(0.95 * rho)
+        initial_reference_set = Bp.intersection(free_rho.buffer(-0.1 * rho))
+
+        if not initial_reference_set.is_empty:
+            break
+
+    # Initial and goal reference position selection
+    r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(p))
+    r0 = np.array(r0_sh.coords[0])
+    rg_sh, _ = shapely.ops.nearest_points(free_rho, shapely.geometry.Point(pg))
+    rg = np.array(rg_sh.coords[0])
+
+
+    # TODO: Check more thoroughly
+    if free_star_prev is not None:
+        free_star_prev = free_star_prev.buffer(-1e-4)
+    if free_star_prev is not None and free_star_prev.contains(r0_sh) and free_star_prev.contains(rg_sh) and free_rho.contains(free_star_prev):# not any([free_star_prev.covers(o.polygon()) for o in obstacles_rho]):
+        if verbosity > 1:
+            print("[Workspace modification]: Reuse workspace from previous time step.")
+        obstacle_clusters = previous_obstacle_clusters
+        exit_flag = 10
+    else:
+        # Apply cluster and starify
+        obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles_rho, r0, rg,
+                                                                                hull_epsilon, max_compute_time-toc(t_init),
+                                                                                previous_clusters=previous_obstacle_clusters,
+                                                                                    make_convex=make_convex, verbose=verbosity)
+
+    free_star = shapely.geometry.box(-20, -20, 20, 20)
+    for o in obstacle_clusters:
+        free_star = free_star.difference(o.polygon())
+
+    compute_time = toc(t_init)
+    return obstacle_clusters, r0, rg, rho, free_star, compute_time, exit_flag
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py
new file mode 100644
index 0000000..e6a308c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/__init__.py
@@ -0,0 +1,4 @@
+from .workspace_modification import workspace_modification
+from .path_generator import path_generator, pol2pos
+from .tunnel_mpc import TunnelMpc
+from .tunnel_mpc_controller import TunnelMpcController
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore
new file mode 100644
index 0000000..86d0cb2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/mpc_build/.gitignore
@@ -0,0 +1,4 @@
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py
new file mode 100644
index 0000000..fae769c
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/path_generator.py
@@ -0,0 +1,108 @@
+import numpy as np
+import time
+import shapely
+from starworlds.utils.misc import tic, toc
+from motion_control.soads.soads import f as soads_f
+
+
+def pol2pos(path_pol, s):
+    n_pol = len(path_pol) // 2 - 1
+    return [sum([path_pol[j * (n_pol + 1) + i] * s ** (n_pol - i) for i in range(n_pol + 1)]) for j in range(2)]
+
+
+def path_generator(r0, rg, obstacles, dp_max, N, dt, max_compute_time, n_pol, ds_decay_rate=0.5,
+                   ds_increase_rate=2., max_nr_steps=1000, convergence_tolerance=1e-5, P_prev=None, s_prev=None,
+                   reactivity=1., crep=1., tail_effect=False, reference_step_size=0.5, verbosity=0):
+    t0 = tic()
+
+    # Initialize
+    ds = 1
+    s = np.zeros(max_nr_steps)
+    r = np.zeros((max_nr_steps, r0.size))
+    if P_prev is not None:
+        i = P_prev.shape[0]
+        r[:i, :] = P_prev
+        s[:i] = s_prev
+    else:
+        i = 1
+        r[0, :] = r0
+
+    while True:
+        dist_to_goal = np.linalg.norm(r[i - 1, :] - rg)
+        # Check exit conditions
+        if dist_to_goal < convergence_tolerance:
+            if verbosity > 1:
+                print("[Path Generator]: Path converged. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+        if s[i - 1] >= N:
+            if verbosity > 1:
+                print("[Path Generator]: Completed path length. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+        if toc(t0) > max_compute_time:
+            if verbosity > 1:
+                print("[Path Generator]: Max compute time in path integrator. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+        if i >= max_nr_steps:
+            if verbosity > 1:
+                print("[Path Generator]: Max steps taken in path integrator. " + str(
+                    int(100 * (s[i - 1] / N))) + "% of path completed.")
+            break
+
+        # Movement using SOADS dynamics
+        dr = min(dp_max, dist_to_goal) * soads_f(r[i - 1, :], rg, obstacles, adapt_obstacle_velocity=False,
+                                                 unit_magnitude=True, crep=crep,
+                                                 reactivity=reactivity, tail_effect=tail_effect,
+                                                 convergence_tolerance=convergence_tolerance)
+
+        r[i, :] = r[i - 1, :] + dr * ds
+
+        ri_in_obstacle = False
+        while any([o.interior_point(r[i, :]) for o in obstacles]):
+            if verbosity > 1:
+                print("[Path Generator]: Path inside obstacle. Reducing integration step from {:5f} to {:5f}.".format(ds, ds*ds_decay_rate))
+            ds *= ds_decay_rate
+            r[i, :] = r[i - 1, :] + dr * ds
+            # Additional compute time check
+            if toc(t0) > max_compute_time:
+                ri_in_obstacle = True
+                break
+        if ri_in_obstacle:
+            continue
+
+        # Update travelled distance
+        s[i] = s[i - 1] + ds
+        # Try to increase step rate again
+        ds = min(ds_increase_rate * ds, 1)
+        # Increase iteration counter
+        i += 1
+
+    r = r[:i, :]
+    s = s[:i]
+
+    # Evenly spaced path
+    s_vec = np.arange(0, s[-1] + reference_step_size, reference_step_size)
+    xs, ys = np.interp(s_vec, s, r[:, 0]), np.interp(s_vec, s, r[:, 1])
+    # Append not finished path with fixed final position
+    s_vec = np.append(s_vec, np.arange(s[-1] + reference_step_size, N + reference_step_size, reference_step_size))
+    xs = np.append(xs, xs[-1] * np.ones(len(s_vec)-len(xs)))
+    ys = np.append(ys, ys[-1] * np.ones(len(s_vec)-len(ys)))
+
+    reference_path = [el for p in zip(xs, ys) for el in p]
+
+    # TODO: Fix when close to goal
+    # TODO: Adjust for short arc length, skip higher order terms..
+    path_pol = np.polyfit(s_vec, reference_path[::2], n_pol).tolist() + \
+               np.polyfit(s_vec, reference_path[1::2], n_pol).tolist()
+    # Force init position to be correct
+    path_pol[n_pol] = reference_path[0]
+    path_pol[-1] = reference_path[1]
+
+    # Compute polyfit approximation error
+    epsilon = [np.linalg.norm(np.array(reference_path[2 * i:2 * (i + 1)]) - np.array(pol2pos(path_pol, s_vec[i]))) for i in
+               range(N + 1)]
+
+    compute_time = toc(t0)
+    return path_pol, epsilon, reference_path, compute_time
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py
new file mode 100644
index 0000000..81c55d9
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc.py
@@ -0,0 +1,386 @@
+import casadi.casadi as casadi
+import opengen as og
+import numpy as np
+import os, sys
+import yaml
+
+
+class NoSolutionError(Exception):
+    '''raise this when there's no solution to the mpc problem'''
+    pass
+
+
+class TunnelMpc:
+
+    def __init__(self, params, robot):
+        self.build_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'mpc_build')
+        # Load parameters
+        self.build_params = None
+        self.robot = robot
+        self.set_build_params(params)
+        rebuild, self.build_name = self.get_build_version()
+        # Build if different from latest build
+        if rebuild:
+            self.build()
+        else:
+            print("Found MPC build: {}".format(self.build_name))
+
+        sys.path.insert(1, os.path.join(self.build_dir, self.build_name))
+        optimizer = __import__(self.build_name)
+        self.solver = optimizer.solver()
+        self.reset()
+
+    def reset(self):
+        self.sol_prev = None
+
+    def set_build_params(self, params):
+        self.build_params = {
+            'mpc_ver': 5,
+            'mode': params['build_mode'],
+            'N': params['N'],
+            'dt': params['dt'],
+            'solver_tol': params['solver_tol'],
+            'solver_max_time': params['solver_max_time'],
+            'solver_max_inner_iterations': params['solver_max_inner_iterations'],
+            'solver_max_outer_iterations': params['solver_max_outer_iterations'],
+            'x_min': self.robot.x_min,
+            'x_max': self.robot.x_max,
+            'u_min': self.robot.u_min,
+            'u_max': self.robot.u_max,
+            'nx': self.robot.nx,
+            'nu': self.robot.nu,
+            'np': 2,
+            'robot_model': self.robot.__class__.__name__,
+            'n_pol': params['n_pol'],
+            'integration_method': params['integration_method']
+        }
+
+    def get_build_version(self):
+        builds = [name for name in os.listdir(self.build_dir)
+                  if os.path.isdir(os.path.join(self.build_dir, name))]
+        for build_ver in builds:
+            if not os.path.isfile(os.path.join(self.build_dir, build_ver, 'build_params.yaml')):
+                continue
+            with open(os.path.join(self.build_dir, build_ver, 'build_params.yaml'), 'r') as file:
+                build_ver_params = yaml.load(file, Loader=yaml.FullLoader)
+            if self.build_params == build_ver_params:
+                return False, build_ver
+        ver = 0
+        while 'ver' + str(ver) in os.listdir(self.build_dir):
+            ver += 1
+        return True, 'ver' + str(ver)
+
+
+    def e_cost(self, e, ce):
+        return ce * e
+
+    def ud_cost(self, u, u_prev, R):
+        ud = [(u[i] - u_prev[i]) for i in range(self.robot.nu)]
+        return R[0] * ud[0] ** 2 + R[1] * ud[1] ** 2
+
+    def ref(self, path_pol, s):
+        return [sum([path_pol[j*(self.build_params['n_pol']+1) + i] * s**(self.build_params['n_pol']-i) for i in range(self.build_params['n_pol']+1)])
+                for j in range(self.build_params['np'])]
+
+    def fz(self, z, mu):
+        return self.robot.f(z[:-1], mu[:-1]) + [mu[-1] / self.build_params['dt']]
+
+    def fz_d(self, z, mu):
+        nz = self.build_params['nx'] + 1
+        if self.build_params['integration_method'] == 'euler':
+            zd = self.fz(z, mu)
+            return [z[i] + zd[i] * self.build_params['dt'] for i in range(nz)]
+        # if self.build_params['integration_method'] == 'RK4':
+        #     k1 = self.fz(z, u, v, path_pol)
+        #     k2 = self.fz([z[i] + self.build_params['dt'] / 2 * k1[i] for i in range(nz)], u, v, path_pol)
+        #     k3 = self.fz([z[i] + self.build_params['dt'] / 2 * k2[i] for i in range(nz)], u, v, path_pol)
+        #     k4 = self.fz([z[i] + self.build_params['dt'] * k3[i] for i in range(nz)], u, v, path_pol)
+        #     return [z[i] + (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) * self.build_params['dt'] / 6 for i in range(nz)]
+        raise NotImplemented
+
+    def one_step_feasible(self, sol, x0, path_pol, e_max, s_kappa, obstacles, d=False):
+        u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol)
+        sol_ok = True
+        eps = 1e-3
+        # x constraints
+        if not all([self.build_params['x_min'][i] - eps <= x[0][i] <= self.build_params['x_max'][i] + eps for i in
+                    range(self.build_params['nx'])]):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad x")
+                print(x[0])
+        # s constraints
+        if not (0 - eps <= s[1] <= self.build_params['N'] + eps):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad s")
+                print(s[1])
+        # u constraints
+        if not all([self.build_params['u_min'][i] - eps <= u[i] <= self.build_params['u_max'][i] + eps for i in
+                    range(self.build_params['nu'])]):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad u")
+                print(u[0])
+        # e constraints
+        if not (e[1] <= e_max + eps):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad e ({:.4f} > {:.4f})".format(e[1], e_max))
+            p = x[0][:self.build_params['np']]
+            if not any([o.interior_point(p) for o in obstacles]):
+                sol_ok = True
+                if d:
+                    print("[MPC]: Position collision free. OK solution.")
+        # ds constraints
+        if not (s_kappa - eps <= ds[0] <= 1 + eps):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad ds ({:.4f} < {:.4f})".format(ds[0], s_kappa))
+
+        return sol_ok
+
+    def is_feasible(self, sol, x0, path_pol, e_max, s_kappa, d=False):
+        u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol)
+        sol_ok = True
+
+        eps = 1e-2
+        # x constraints
+        if not all([self.build_params['x_min'][i]-eps <= xk[i] <= self.build_params['x_max'][i]+eps for i in range(self.build_params['nx']) for xk in x]):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad x")
+                print(x)
+        # s constraints
+        if not all([0-eps <= sk <= self.build_params['N'] + eps for sk in s]):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad s")
+                print(s)
+        # u constraints
+        if not all([self.build_params['u_min'][i]-eps <= uk <= self.build_params['u_max'][i]+eps for i in range(self.build_params['nu']) for uk in u[i::self.build_params['nu']]]):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad u")
+                print(u)
+        # v constraints
+        if not all([0-eps <= dsk <= np.inf * self.build_params['dt'] + eps for dsk in ds] or ds[0]) < s_kappa:
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad ds")
+                print(ds)
+                print(s_kappa)
+        # e constraints
+        if not all([ek <= e_max + eps for ek in e]):
+            sol_ok = False
+            if d:
+                print("[MPC]: Bad e")
+                print(e)
+
+        # All constraints ok
+        return sol_ok
+
+    def e_sqr(self, z, path_pol):
+        ref = self.ref(path_pol, z[-1])
+        p = self.robot.h(z[:-1])
+        e_sqr = sum([(ref[i] - p[i])**2 for i in range(self.build_params['np'])])
+        return e_sqr
+
+    def unpack_solution(self, sol, x0, path_pol):
+        u = sol[:self.build_params['nu'] * self.build_params['N']]
+        ds = sol[self.build_params['nu'] * self.build_params['N']:]
+        zk = x0 + [0]
+        x = [zk[:-1]]
+        s = [zk[-1]]
+        e = [self.e_sqr(zk, path_pol)]
+        for k in range(self.build_params['N']):  # LOOP OVER TIME STEPS
+            # Dynamics
+            muk = u[self.build_params['nu'] * k:self.build_params['nu'] * (k + 1)] + [ds[k]]
+            zk = self.fz_d(zk, muk)
+            x += [zk[:-1]]
+            s += [zk[-1]]
+            e += [np.sqrt(self.e_sqr(zk, path_pol))]
+        return u, ds, x, s, e
+
+    def sol2cost(self, sol, x0, path_pol, u_prev, params):
+        u, ds, x, s, e = self.unpack_solution(sol, x0, path_pol)
+        u = u_prev + u
+        v_cost, ud_cost, omega_cost = [0.] * 3
+        for i in range(len(ds)):
+            ui_prev = u[self.build_params['nu'] * i:self.build_params['nu'] * (i + 1)]
+            ui = u[self.build_params['nu'] * (i+1):self.build_params['nu'] * (i + 2)]
+            ud_cost += self.ud_cost(ui, ui_prev, params['R'])
+            v_cost += self.v_cost(ds[i], params['cv'], params['lambda'], i)
+        e_end_cost = self.e_cost(e[-1]**2, params['ce_end'])
+        return v_cost, ud_cost, e_end_cost, omega_cost
+
+    def build(self):
+        # Build parametric optimizer
+        # ------------------------------------
+        params = {'x0': self.build_params['nx'], 'u_prev': self.build_params['nu'], 'path_pol': self.build_params['np'] * (self.build_params['n_pol']+1),
+                  'cs': 1, 'ce': 1, 'R': self.build_params['nu'], 'e_max': 1, 'rg': self.build_params['np'], 'e_penalty': 1, 'cg': 1, 's_kappa': 1}
+        par_dim = sum(list(params.values()))
+
+
+        # Exchange parameter dimension with value
+        par = casadi.SX.sym("par", par_dim)  # Parameters
+        p_idx = 0
+        for key, dim in params.items():
+            params[key] = par[p_idx:p_idx + dim]
+            p_idx += dim
+
+        # Init value
+        uk_prev = params['u_prev']
+        zk = casadi.vertcat(params['x0'], 0)
+        cost = 0
+        # Decision variable
+        mu = casadi.SX.sym('uds', (self.build_params['nu'] + 1) * self.build_params['N'])
+        # Constraint variables
+        f1, f1_min, f1_max = [], [], []
+
+        # Loop over time steps
+        for k in range(self.build_params['N']):
+            # Decision variables
+            uk = mu[k * self.build_params['nu']:(k + 1) * self.build_params['nu']]
+            dsk = mu[self.build_params['nu'] * self.build_params['N'] + k]
+            muk = casadi.vertcat(uk, dsk)
+
+            # Step forward
+            zk1 = casadi.vertcat(*self.fz_d(zk, muk))
+            ek1_sqr = self.e_sqr(zk1, params['path_pol'])
+
+            # Add stage cost
+            cost += params['e_penalty'] * ek1_sqr
+            cost += self.ud_cost(uk, uk_prev, params['R'])
+
+            # Add state and error constraint
+            f1 = casadi.vertcat(f1, zk1, ek1_sqr - params['e_max']**2)
+            f1_min += self.build_params['x_min'] + [0] + [-1e10]
+            f1_max += self.build_params['x_max'] + [self.build_params['N']] + [0]
+
+            # Initial reference increment constraint
+            if k == 0:
+                f1 = casadi.vertcat(f1, dsk - params['s_kappa'])
+                f1_min += [0]
+                f1_max += [1e10]
+
+            uk_prev = uk
+            zk = zk1
+
+
+        ## Terminal cost
+        # Cost on last tracking error
+        cost += self.e_cost(ek1_sqr, params['ce'])
+        cost += - params['cs'] * zk1[-1]
+        # Convergence cost
+        p_end = self.robot.h(zk[:-1])
+        xg_err = sum([(p_end[i] - params['rg'][i])**2 for i in range(self.build_params['np'])])
+        cost += params['cg'] * xg_err
+        # Final orientation cost
+        # cost += params['cg_orientation'] * (1 / (xg_err + 1e-3) ) * (zk[2]-params['rg'][2])**2
+
+        # Constraint set
+        set_c = og.constraints.Rectangle(f1_min, f1_max)
+        # Decision variable constraints
+        mu_bounds = og.constraints.Rectangle(self.build_params['u_min'] * self.build_params['N'] +
+                                               [0.] * self.build_params['N'],
+                                               self.build_params['u_max'] * self.build_params['N'] +
+                                               [1] * self.build_params['N'])
+
+        # Setup builder
+        problem = og.builder.Problem(mu, par, cost) \
+            .with_constraints(mu_bounds) \
+            .with_aug_lagrangian_constraints(f1, set_c)
+        build_config = og.config.BuildConfiguration() \
+            .with_build_directory(self.build_dir) \
+            .with_build_mode(self.build_params['mode']) \
+            .with_build_python_bindings()
+        meta = og.config.OptimizerMeta() \
+            .with_optimizer_name(self.build_name)
+        solver_config = og.config.SolverConfiguration() \
+            .with_tolerance(self.build_params['solver_tol']) \
+            .with_max_duration_micros(self.build_params['solver_max_time'] * 1000) \
+            .with_max_inner_iterations(self.build_params['solver_max_inner_iterations']) \
+            .with_max_outer_iterations(self.build_params['solver_max_outer_iterations'])
+        builder = og.builder.OpEnOptimizerBuilder(problem,
+                                                  meta,
+                                                  build_config,
+                                                  solver_config) \
+            .with_verbosity_level(1)
+        builder.build()
+        print('')
+
+        # Save build params
+        with open(os.path.join(self.build_dir, self.build_name, 'build_params.yaml'), 'w') as file:
+            yaml.dump(self.build_params, file)
+
+    def trivial_solution(self, x, path_pol, e_max, s_kappa):
+        p_ref = self.ref(path_pol, s_kappa)
+
+        u = [0] * (self.build_params['N'] * self.build_params['nu'])
+        ds = [0] * (self.build_params['N'])
+        xk = x
+        sk = 0
+        zk = xk + [sk]
+        for k in range(self.build_params['N']):  # LOOP OVER TIME STEPS
+            # Control law
+            if self.build_params['robot_model'] == 'Unicycle':
+                ref_dist = np.linalg.norm(np.array(xk[:2])-np.array(p_ref))
+                if ref_dist > 1e-5:
+                    theta_ref = np.arctan2(p_ref[1]-xk[1], p_ref[0]-xk[0])
+                    theta_diff = theta_ref - xk[-1]
+                    if theta_diff > np.pi:
+                        theta_diff -= 2 * np.pi
+                    if theta_diff < -np.pi:
+                        theta_diff += 2 * np.pi
+                    if abs(theta_diff) < 1e-2: # Less than 0.57 degree error
+                        # Linear velocity
+                        u[k*self.build_params['nu']] = min(self.build_params['u_max'][0], ref_dist / self.build_params['dt'])
+                    else:
+                        # Angular velocity
+                        if theta_diff > 0:
+                            u[k * self.build_params['nu'] + 1] = min(self.build_params['u_max'][1], theta_diff / self.build_params['dt'])
+                        else:
+                            u[k * self.build_params['nu'] + 1] = max(self.build_params['u_min'][1], theta_diff / self.build_params['dt'])
+
+            # Dynamics
+            muk = u[k * self.build_params['nu']:k * self.build_params['nu'] + 2] + [0]
+            zk = self.fz_d(zk, muk)
+            xk = zk[:-1]
+
+        return u + ds
+
+    def run(self, x, u_prev, path_pol, params, e_max, rg, s_kappa, obstacles, verbosity=0):
+        robot_within_dg = np.linalg.norm(np.array(self.robot.h(rg))-np.array(self.robot.h(x))) < params['dg']
+        cg = params['cg'] if robot_within_dg else 0
+
+        p = x + u_prev + path_pol + [params['cs'], params['ce']] + params['R'] + \
+            [0.95*e_max] + rg + [params['e_penalty'], cg, s_kappa]
+
+        if self.sol_prev is None or not self.is_feasible(self.sol_prev, x, path_pol, 2*e_max, s_kappa):
+            # Use trivial solution as initial guess
+            self.sol_prev = self.trivial_solution(x, path_pol, e_max, s_kappa)
+        # Run solver
+        solution_data = self.solver.run(p=p, initial_guess=self.sol_prev)
+
+        # TODO: When not one step feasible, check for actual collision with mpc before using trivial solution.
+        if solution_data is None or not self.one_step_feasible(solution_data.solution, x, path_pol, e_max, s_kappa, obstacles, d=verbosity>0):
+            if verbosity > 0:
+                print("[MPC]: Not feasible solution. Using trivial solution.")
+            sol = self.trivial_solution(x, path_pol, e_max, s_kappa)
+            self.sol_prev = None
+            exit_status = "Trivial solution"
+            cost = -1
+        else:
+            sol = solution_data.solution
+            exit_status = solution_data.exit_status
+            cost = solution_data.cost
+
+            self.sol_prev = sol[self.build_params['nu']:self.build_params['nu'] * self.build_params['N']] + \
+                            sol[self.build_params['nu'] * (self.build_params['N']-1):self.build_params['nu'] * self.build_params['N']] \
+                            + sol[self.build_params['nu'] * self.build_params['N']:]
+
+        # Unpack solution
+        u, ds, x_pred, s_pred, e_pred = self.unpack_solution(sol, x, path_pol)
+
+        return {'u': u, 'x': x_pred, 'e': e_pred, 's': s_pred, 'cost': cost, 'exit_status': exit_status}
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py
new file mode 100644
index 0000000..8ca8b2f
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/tunnel_mpc_controller.py
@@ -0,0 +1,107 @@
+from motion_control.tunnel_mpc_convergence import workspace_modification, path_generator, pol2pos, TunnelMpc
+from starworlds.utils.misc import tic, toc
+import numpy as np
+
+
+class TunnelMpcController:
+    def __init__(self, params, robot, verbosity=0):
+        self.params = params
+        self.params['dp_max'] = robot.vmax * self.params['dt']
+        self.robot = robot
+        self.mpc = TunnelMpc(params, robot)
+        self.verbosity = verbosity
+        self.reset()
+
+    def reset(self):
+        self.mpc.reset()
+        self.obstacle_clusters = None
+        self.obstacles_star = []
+        self.free_star = None
+        self.ri = None
+        self.rg = None
+        self.rs = None
+        self.target_path = []
+        self.path_pol = None
+        self.rho = None
+        self.epsilon = None
+        self.solution = None
+        self.u_prev = [0] * self.robot.nu
+        self.timing = {'workspace': 0, 'target': 0, 'mpc': 0}
+
+    def compute_u(self, x, pg, obstacles):
+        p = self.robot.h(x)
+
+        # Initialize rs to robot position
+        if self.rs is None:
+            self.rs = p
+        # Update obstacles
+        if not self.params['use_prev_workspace']:
+            self.free_star = None
+        self.obstacle_clusters, self.ri, self.rg, self.rho, self.free_star, self.timing['workspace'], exit_flag = \
+            workspace_modification(obstacles, p, pg, self.rs, self.params['rho0'], self.params['max_obs_compute_time'],
+                                   self.params['hull_epsilon'], self.params['gamma'],
+                                   make_convex=self.params['make_convex'], previous_obstacle_clusters=self.obstacle_clusters,
+                                   free_star_prev=self.free_star, verbosity=self.verbosity)
+        self.obstacles_star = [o.cluster_obstacle for o in self.obstacle_clusters]
+
+        # Buffer previous target path
+        P_prev, s_prev = None, None
+        if self.params['buffer'] and self.target_path:
+            P_prev = np.array([self.target_path[::2], self.target_path[1::2]]).T
+            # Shift path to start closest to current rs
+            P_prev = P_prev[np.argmin(np.linalg.norm(self.rs - P_prev, axis=1)):, :]
+            P_prev[0, :] = self.rs
+
+            # ||self.rs - p|| < rho from construction
+            for r in P_prev:
+                if any([o.interior_point(r) for o in self.obstacles_star]):
+                    if self.verbosity > 0:
+                        print("[Path Generator]: No reuse of previous path. Path not collision-free.")
+                    P_prev = None
+                    break
+
+            #     P_prev = None
+            if P_prev is not None:
+                # Cut off stand still padding in previous path
+                P_prev_stepsize = np.linalg.norm(np.diff(P_prev, axis=0), axis=1)
+                s_prev = np.hstack((0, np.cumsum(P_prev_stepsize) / self.params['dp_max']))
+                P_prev_mask = [True] + (P_prev_stepsize > 1e-8).tolist()
+                P_prev = P_prev[P_prev_mask, :]
+                s_prev = s_prev[P_prev_mask]
+
+        # Make sure all polygon representations are computed
+        [o._compute_polygon_representation() for o in self.obstacles_star]
+
+        # Check for convergence
+        if np.linalg.norm(np.array(p) - np.array(pg)) < self.params['convergence_margin']:
+            self.timing['target'] = 0
+            self.timing['mpc'] = 0
+            return np.zeros(self.robot.nu)
+
+        # Update target path
+        self.path_pol, self.epsilon, self.target_path, self.timing['target'] = \
+            path_generator(self.ri, self.rg, self.obstacles_star, self.params['dp_max'], self.params['N'],
+                           self.params['dt'], self.params['max_compute_time'], self.params['n_pol'],
+                           ds_decay_rate=0.5, ds_increase_rate=2., max_nr_steps=1000, P_prev=P_prev, s_prev=s_prev,
+                           reactivity=self.params['reactivity'], crep=self.params['crep'],
+                           convergence_tolerance=self.params['convergence_tolerance'], verbosity=self.verbosity)
+
+        # Compute MPC solution
+        t0 = tic()
+        # Set parameter for tracking error constraint
+        if self.rho > 0:
+            e_max = self.rho - max(self.epsilon)
+        else:
+            e_max = 1.e6
+
+        self.s_kappa = self.params['lambda'] * self.rho / self.params['dp_max']
+        self.solution = self.mpc.run(x.tolist(), self.u_prev, self.path_pol, self.params, e_max, self.rg.tolist(), self.s_kappa, obstacles, verbosity=self.verbosity)
+        self.u_prev = self.solution['u'][:self.robot.nu]
+        self.timing['mpc'] = toc(t0)
+
+        # Update rs
+        self.rs = self.mpc.ref(self.path_pol, self.solution['s'][1])
+
+        # Extract first control signal
+        return np.array(self.solution['u'][:self.mpc.build_params['nu']])
+
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py
new file mode 100644
index 0000000..920277d
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/motion_control/tunnel_mpc_convergence/workspace_modification.py
@@ -0,0 +1,66 @@
+import numpy as np
+from starworlds.starshaped_hull import cluster_and_starify, ObstacleCluster
+from starworlds.utils.misc import tic, toc
+import shapely
+
+# TODO: Check why computational time varies so much for same static scene
+def workspace_modification(obstacles, p, pg, rs, rho0, max_compute_time, hull_epsilon, gamma=0.5, make_convex=0,
+                            previous_obstacle_clusters=None, free_star_prev=None, verbosity=0):
+
+    # Clearance variable initialization
+    rho = rho0 / gamma  # First rho should be rho0
+    t_init = tic()
+
+    while True:
+        if toc(t_init) > max_compute_time:
+            if verbosity > 0:
+                print("[Workspace modification]: Max compute time in rho iteration.")
+            break
+
+        # Reduce rho
+        rho *= gamma
+
+        # Pad obstacles with rho
+        obstacles_rho = [o.dilated_obstacle(padding=rho, id="duplicate") for o in obstacles]
+
+        # TODO: Fix boundaries
+        free_rho = shapely.geometry.box(-20, -20, 20, 20)
+        for o in obstacles_rho:
+            free_rho = free_rho.difference(o.polygon())
+
+        # TODO: Check buffering fix
+        # Find P0
+        Bp = shapely.geometry.Point(p).buffer(0.95 * rho)
+        initial_reference_set = Bp.intersection(free_rho.buffer(-0.1 * rho))
+
+        if not initial_reference_set.is_empty:
+            break
+
+    # Initial and goal reference position selection
+    r0_sh, _ = shapely.ops.nearest_points(initial_reference_set, shapely.geometry.Point(rs))
+    r0 = np.array(r0_sh.coords[0])
+    rg_sh, _ = shapely.ops.nearest_points(free_rho, shapely.geometry.Point(pg))
+    rg = np.array(rg_sh.coords[0])
+
+
+    # TODO: Check more thoroughly
+    if free_star_prev is not None:
+        free_star_prev = free_star_prev.buffer(-1e-4)
+    if free_star_prev is not None and free_star_prev.contains(r0_sh) and free_star_prev.contains(rg_sh) and free_rho.contains(free_star_prev):# not any([free_star_prev.covers(o.polygon()) for o in obstacles_rho]):
+        if verbosity > 1:
+            print("[Workspace modification]: Reuse workspace from previous time step.")
+        obstacle_clusters = previous_obstacle_clusters
+        exit_flag = 10
+    else:
+        # Apply cluster and starify
+        obstacle_clusters, obstacle_timing, exit_flag, n_iter = cluster_and_starify(obstacles_rho, r0, rg,
+                                                                                hull_epsilon, max_compute_time-toc(t_init),
+                                                                                previous_clusters=previous_obstacle_clusters,
+                                                                                    make_convex=make_convex, verbose=verbosity)
+
+    free_star = shapely.geometry.box(-20, -20, 20, 20)
+    for o in obstacle_clusters:
+        free_star = free_star.difference(o.polygon())
+
+    compute_time = toc(t_init)
+    return obstacle_clusters, r0, rg, rho, free_star, compute_time, exit_flag
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py
new file mode 100644
index 0000000..a0f0b8a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/__init__.py
@@ -0,0 +1,7 @@
+"""
+The :mod:`robot` module implements mobile robots for obstacle avoidance.
+"""
+from .mobile_robot import MobileRobot
+from .unicycle import Unicycle
+from .bicycle import Bicycle
+from .omnidirectional import Omnidirectional
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py
new file mode 100644
index 0000000..2aa541a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/bicycle.py
@@ -0,0 +1,55 @@
+import numpy as np
+from robot import MobileRobot
+
+
+class Bicycle(MobileRobot):
+
+    def __init__(self, width, vel_min=None, vel_max=None, steer_max=None, name='robot'):
+        self.vmax = vel_max[0]
+        x_min = [-np.inf] * 3 + [-steer_max]
+        x_max = [np.inf] * 3 + [steer_max]
+        super().__init__(nu=2, nx=4, width=width, name=name, u_min=vel_min, u_max=vel_max, x_min=x_min, x_max=x_max)
+
+    # [x,y of backwheel, orientation, steering angle]
+    # def f(self, x, u):
+    #     return [u[0] * np.cos(x[2]),
+    #             u[0] * np.sin(x[2]),
+    #             u[0] * np.tan(u[1]) / self.width]
+    def f(self, x, u):
+        return [u[0] * np.cos(x[2]),
+                u[0] * np.sin(x[2]),
+                u[0] * np.tan(u[1]) / self.width,
+                u[1]]
+    # def f(self, x, u):
+    #     return [u[0] * np.cos(x[2]) * np.cos(x[3]),
+    #             u[0] * np.sin(x[2]) * np.cos(x[3]),
+    #             u[0] * np.sin(x[3]) / self.width,
+    #             u[1]]
+
+    def h(self, x):
+        return [x[0] + self.width/2 * np.cos(x[2]),
+                x[1] + self.width/2 * np.sin(x[2])]
+
+    def vel_min(self):
+        return self.u_min
+
+    def vel_max(self):
+        return self.u_max
+
+    def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10):
+        h = super(Bicycle, self).init_plot(ax=ax, color=color, alpha=alpha)
+        h += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color)
+        h += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w')
+        h += ax.plot(0, 0, color=color, alpha=alpha)
+        return h
+
+    def update_plot(self, x, handles):
+        super(Bicycle, self).update_plot(x, handles)
+        handles[1].set_data(self.h(x))
+        handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[1].set_markersize(handles[1].get_markersize())
+        handles[2].set_data(x[0], x[1])
+        handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[2].set_markersize(handles[2].get_markersize())
+        handles[3].set_data([x[0], x[0] + self.width * np.cos(x[2])],
+                            [x[1], x[1] + self.width * np.sin(x[2])])
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py
new file mode 100644
index 0000000..759a37e
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/mobile_robot.py
@@ -0,0 +1,62 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from abc import ABC, abstractmethod
+import numpy as np
+
+
+class MobileRobot(ABC):
+
+    def __init__(self, nu, nx, width, name, u_min=None, u_max=None, x_min=None, x_max=None):
+        self.nx = nx
+        self.nu = nu
+        self.width = width
+        self.name = name
+        def valid_u_bound(bound): return bound is not None and len(bound) == self.nu
+        def valid_q_bound(bound): return bound is not None and len(bound) == self.nx
+        self.u_min = u_min if valid_u_bound(u_min) else [-np.inf] * self.nu
+        self.u_max = u_max if valid_u_bound(u_max) else [np.inf] * self.nu
+        self.x_min = x_min if valid_q_bound(x_min) else [-np.inf] * self.nx
+        self.x_max = x_max if valid_q_bound(x_max) else [np.inf] * self.nx
+
+    @abstractmethod
+    def f(self, x, u):
+        pass
+
+    @abstractmethod
+    def h(self, q):
+        pass
+
+    @abstractmethod
+    def vel_min(self):
+        pass
+
+    @abstractmethod
+    def vel_max(self):
+        pass
+
+    def move(self, x, u, dt):
+        u_sat = np.clip(u, self.u_min, self.u_max)
+        x_next = x + np.array(self.f(x, u_sat)) * dt
+        x_next = np.clip(x_next, self.x_min, self.x_max)
+        return x_next, u_sat
+
+    def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10):
+        if ax is None:
+            _, ax = plt.subplots(subplot_kw={'aspect': 'equal'})
+        if self.width == 0:
+            handles = plt.plot(0, 0, '+', color=color, alpha=alpha, markersize=markersize, label=self.name)
+        else:
+            handles = [patches.Circle((0, 0), self.width / 2, color=color, alpha=alpha, label=self.name)]
+            ax.add_patch(handles[0])
+        return handles, ax
+
+    def update_plot(self, x, handles):
+        if self.width == 0:
+            handles[0].set_data(*self.h(x))
+        else:
+            handles[0].set(center=self.h(x))
+
+    def draw(self, x, **kwargs):
+        handles, ax = self.init_plot(**kwargs)
+        self.update_plot(x, handles)
+        return handles, ax
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py
new file mode 100644
index 0000000..52592b3
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/omnidirectional.py
@@ -0,0 +1,23 @@
+import numpy as np
+from robot import MobileRobot
+
+
+class Omnidirectional(MobileRobot):
+
+    def __init__(self, width, vel_max, name='robot'):
+        vel_max = vel_max * np.ones(2)
+        self.vmax = np.linalg.norm(vel_max)
+        super().__init__(nu=2, nx=2, width=width, name=name, u_min=(-vel_max).tolist(), u_max=(vel_max).tolist())
+
+    def f(self, x, u):
+        return [u[0],
+                u[1]]
+
+    def h(self, x):
+        return x[:2]
+
+    def vel_min(self):
+        return self.u_min
+
+    def vel_max(self):
+        return self.u_max
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py
new file mode 100644
index 0000000..2891df2
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/robot/unicycle.py
@@ -0,0 +1,38 @@
+import numpy as np
+from robot import MobileRobot
+
+
+class Unicycle(MobileRobot):
+
+    def __init__(self, width, vel_min=None, vel_max=None, name='robot'):
+        self.vmax = vel_max[0]
+        super().__init__(nu=2, nx=3, width=width, name=name, u_min=vel_min, u_max=vel_max)
+
+    def f(self, x, u):
+        return [u[0] * np.cos(x[2]),
+                u[0] * np.sin(x[2]),
+                u[1]]
+
+    def h(self, x):
+        return x[:2]
+
+    def vel_min(self):
+        return self.u_min
+
+    def vel_max(self):
+        return self.u_max
+
+    def init_plot(self, ax=None, color='b', alpha=0.7, markersize=10):
+        handles, ax = super(Unicycle, self).init_plot(ax=ax, color=color, alpha=alpha)
+        handles += ax.plot(0, 0, marker=(3, 0, np.rad2deg(0)), markersize=markersize, color=color)
+        handles += ax.plot(0, 0, marker=(2, 0, np.rad2deg(0)), markersize=0.5*markersize, color='w')
+        return handles, ax
+
+    def update_plot(self, x, handles):
+        super(Unicycle, self).update_plot(x, handles)
+        handles[1].set_data(x[0], x[1])
+        handles[1].set_marker((3, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[1].set_markersize(handles[1].get_markersize())
+        handles[2].set_data(x[0], x[1])
+        handles[2].set_marker((2, 0, np.rad2deg(x[2]-np.pi/2)))
+        handles[2].set_markersize(handles[2].get_markersize())
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py
new file mode 100644
index 0000000..821dd31
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/setup.py
@@ -0,0 +1,15 @@
+from setuptools import setup, find_packages
+
+setup(name='starworld_tunnel_mpc',
+      version='1.0',
+      packages=find_packages(),
+      install_requires=[
+          'pyyaml',
+          'numpy',
+          'scipy',
+          'matplotlib',
+          'shapely',
+          'casadi',
+          'opengen'
+      ]
+)
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py
new file mode 100755
index 0000000..c596487
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_motion_control.py
@@ -0,0 +1,75 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from config.load_config import load_config
+from visualization import TunnelMPCGUI
+
+ctrl_param_file = 'tunnel_mpc_params.yaml'
+ctrl_param_file = 'tunnel_mpc_convergence_params.yaml'
+scene_id = None
+verbosity = 0
+
+scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=1, scene_id=scene_id, verbosity=verbosity)
+gui = TunnelMPCGUI(robot, scene.obstacles, x0, scene.pg, scene.xlim, scene.ylim,
+                   mpc_horizon=controller.params['N'], rho0=controller.params['rho0'], # Comment out to skip mpc solution figure
+                   robot_alpha=0., mpc_path_linestyle='--', mpc_path_linewidth=4, target_path_linewidth=2,
+                   goal_color='k', robot_color='orange')
+
+# Initialize
+T_max = 30
+dt = controller.params['dt']
+t = 0.
+x = x0
+u_prev = np.zeros(robot.nu)
+convergence_threshold = 0.05
+converged = False
+timing = {'workspace': [], 'target': [], 'mpc': [], 'tot': []}
+
+while gui.fig_open and not converged:
+
+    if t < T_max and (not gui.paused or gui.step_once):
+        gui.step_once = False
+        p = robot.h(x)
+        # Move obstacles
+        scene.step(dt, np.array([p]), robot.width/2)
+        # Compute mpc
+        u = controller.compute_u(x, scene.pg, scene.obstacles)
+
+        # Update timing
+        tot_time = 0
+        for k, v in controller.timing.items():
+            timing[k] += [v]
+            tot_time += v
+        timing['tot'] += [tot_time]
+
+        # Update plots
+        mpc_path = [el for x in controller.solution['x'] for el in robot.h(x)]
+        e_max = controller.rho - max(controller.epsilon)
+        s_kappa = controller.s_kappa if hasattr(controller,'s_kappa') else None
+        gui.update(robot_state=x, obstacles_star=controller.obstacles_star, time=t, target_path=controller.target_path,
+                   mpc_path=mpc_path, e_max=e_max, timing=controller.timing, e=controller.solution['e'],
+                   s=controller.solution['s'], u=u_prev.tolist() + controller.solution['u'], rho=controller.rho, s_kappa=s_kappa)
+
+        # Integrate robot state with new control signal
+        x, _ = robot.move(x, u, dt)
+        u_prev = u
+        t += dt
+
+    converged = np.linalg.norm(robot.h(x)-scene.pg) < convergence_threshold
+
+    if t >= T_max or converged:
+        gui.ax.set_title("Time: {:.1f} s. Finished".format(t))
+        gui.fig.canvas.draw()
+
+    plt.pause(0.005)
+
+gui.update(robot_state=x, obstacles_star=controller.obstacles_star)
+
+# fig, ax = plt.subplots()
+# ax.plot(timing['workspace'])
+# ax.plot(timing['target'])
+# ax.plot(timing['mpc'])
+# ax.plot(timing['tot'])
+# ax.legend(['Workspace modification', 'Target path generation', 'MPC', 'Total'])
+
+# Wait until figure closed when converged
+plt.show()
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py
new file mode 100755
index 0000000..0b5f404
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/tests/test_soads.py
@@ -0,0 +1,176 @@
+import numpy as np
+import matplotlib.pyplot as plt
+# from config.load_config import load_config
+# from videos.video_writer import VideoWriter
+from motion_control.soads import SoadsController
+from config.load_config import load_config
+
+make_video = 0
+
+ctrl_param_file = 'soads_ctrl_params.yaml'
+scene, robot, controller, x0 = load_config(ctrl_param_file=ctrl_param_file, robot_type_id=0)
+controller.verbose = True
+
+# Initialize
+T_max = 30
+dt = controller.params['dt']
+K = int(T_max / dt)
+u = np.zeros((robot.nu, K))
+x = np.zeros((x0.size, K+1))
+x[:, 0] = x0
+pg = scene.pg
+timing_history = {'obstacle': [], 'control': []}
+previous_path = []
+convergence_threshold = 0.01
+converged = False
+paused = True
+step_once = False
+i = 0
+
+# Init video writing
+if make_video:
+    video_name = "mpc_" + str(scene.id) + "_" + robot.__class__.__name__
+    video_name = input("Video file name: ")
+    video_writer = VideoWriter(video_name)
+    paused = False
+
+# Init plotting
+fig_scene, ax_scene = plt.subplots()
+ax_scene.set_xlabel('x1 [m]', fontsize=15)
+ax_scene.set_ylabel('x2 [m]', fontsize=15)
+travelled_path_handle = ax_scene.plot([], [], 'k-', linewidth=2)[0]
+goal_handle = ax_scene.plot(*scene.pg, 'g*', markersize=16)[0]
+obstacle_handles, _ = scene.init_plot(ax=ax_scene, draw_p0=0, draw_pg=0, draw_streamlines=0, show_obs_name=0)
+scene.update_plot(obstacle_handles)
+robot.width = 0
+robot_handles, _ = robot.init_plot(ax=ax_scene, color='y', alpha=1, markersize=16)
+robot.update_plot(x0, robot_handles)
+obstacle_star_handles = []
+color_list = plt.cm.gist_ncar(np.linspace(0, 1, len(scene.obstacles)))
+
+streamplot_handle = None
+
+fig_open = True
+
+def on_close(event):
+    global fig_open
+    fig_open = False
+
+def on_press(event):
+    global paused, step_once, streamplot_handle
+    if event.key == ' ':
+        paused = not paused
+    elif event.key == 'right':
+        step_once = True
+        paused = True
+    elif event.key == 't':
+        fig_timing, ax = plt.subplots()
+        ax.plot(timing_history['obstacle'], '-o', label='obstacle')
+        ax.plot(timing_history['control'], '-o', label='control')
+        fig_timing.canvas.draw()
+    elif event.key == 'w':
+        ax_scene.axis('off')
+        ax_scene.title.set_visible(False)
+        file_name = input("-------------\nFile name: ")
+        fig_scene.savefig("utils/" + file_name, transparent=True)
+        ax_scene.axis('on')
+        ax_scene.title.set_visible(True)
+    elif event.key == 'a':
+        from motion_control.path_generator import plot_vector_field
+        n = int(input("-------------\nStreamplot resolution: "))
+        streamplot_handle = plot_vector_field(scene.pg, controller.obstacles_star, ax_scene, n=n, color='orange')
+    else:
+        print(event.key)
+
+def remove_local_plots():
+    global streamplot_handle
+    # Remove streamplot
+    if streamplot_handle is not None:
+        from matplotlib.patches import FancyArrowPatch
+        streamplot_handle.lines.remove()
+        for art in ax_scene.get_children():
+            if not isinstance(art, FancyArrowPatch):
+                continue
+            art.remove()  # Method 1
+        streamplot_handle = None
+
+fig_scene.canvas.mpl_connect('close_event', on_close)
+fig_scene.canvas.mpl_connect('key_press_event', on_press)
+
+ls = []
+
+while fig_open and not converged:
+
+    if i < K and (not paused or step_once):
+        p = robot.h(x[:, i])
+        step_once = False
+        # Move obstacles
+        scene.step(dt, np.array([p]), 1e-2)
+        # Compute mpc
+        u[:, i] = controller.compute_u(x[:, i], pg, scene.obstacles)
+        # Integrate robot state with new control signal
+        x[:, i+1], _ = robot.move(x[:, i], u[:, i], dt)
+        # Add timing to history
+        for k in timing_history.keys():
+            timing_history[k] += [controller.timing[k]]
+
+        # Update plots
+        robot.update_plot(x[:, i], robot_handles)
+        scene.update_plot(obstacle_handles)
+        [h.remove() for h in obstacle_star_handles if h is not None]
+        obstacle_star_handles = []
+        for j, cl in enumerate(controller.obstacle_clusters):
+            for o in cl.obstacles:
+                lh, _ = o.draw(ax=ax_scene, fc='lightgrey', show_reference=False)
+                obstacle_star_handles += lh
+        for o in controller.obstacles_star:
+            lh, _ = o.draw(ax=ax_scene, fc='red', show_reference=False, alpha=0.9, zorder=0)
+            obstacle_star_handles += lh
+            lh = ax_scene.plot(*o.xr(), 'gd', markersize=8)
+            obstacle_star_handles += lh
+
+        # if ls:
+        #     [l.remove() for l in ls]
+        # ls = []
+        # for o in controller.obstacles_star:
+        #     b = o.boundary_mapping(x[:, i])
+        #     dx_o = f(x[:, i], pg, [o])
+        #     ls += ax_scene.plot(*b, 'yx')
+        #     ls += [ax_scene.quiver(*b, *dx_o, zorder=3, color='y')]
+
+        travelled_path_handle.set_data(x[0, :i+1], x[1, :i+1])
+
+        ax_scene.set_title("Time: {:.1f} s".format(i*dt))
+
+        i += 1
+
+        if make_video:
+            video_writer.add_frame(fig_scene)
+            if not i % 10:
+                print(f'[VideoWriter] wrote frame {i}/{K}')
+        else:
+            fig_scene.canvas.draw()
+
+    converged = np.linalg.norm(robot.h(x[:, i])-pg) < convergence_threshold
+
+    if i == K or converged:
+        if make_video:
+            # close video writer
+            video_writer.release()
+            print("Finished")
+            fig_open = False
+        else:
+            ax_scene.set_title("Time: {:.1f} s. Finished".format(i * dt))
+            fig_scene.canvas.draw()
+
+    if not make_video:
+        plt.pause(0.005)
+
+
+ot = timing_history['obstacle']
+print("Timing\n-----\nMean: {:.2f}\nMax: {:.2f}\nStdDev: {:.2f}".format(np.mean(ot), np.max(ot), np.std(ot)))
+
+# Wait until figure closed when converged
+# if fig_open:
+if not make_video:
+    plt.show()
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py
new file mode 100644
index 0000000..2a52f2a
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/__init__.py
@@ -0,0 +1 @@
+from .scene_gui import SceneGUI, SoadsGUI, TunnelMPCGUI
\ No newline at end of file
diff --git a/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py
new file mode 100644
index 0000000..bc811eb
--- /dev/null
+++ b/python/ur_simple_control/path_generation/starworld_tunnel_mpc/visualization/scene_gui.py
@@ -0,0 +1,182 @@
+import matplotlib.pyplot as plt
+import shapely
+import numpy as np
+
+
+class SceneGUI:
+
+    def __init__(self, robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis=False,
+                 robot_color='k', robot_markersize=14, robot_alpha=0.7,
+                 obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False,
+                 goal_color='y', goal_marker='*', goal_markersize=16,
+                 travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2):
+        self.obstacles = obstacles
+        self.robot = robot
+        self.fig, self.ax = plt.subplots()
+        self.ax.set_xlim(xlim), self.ax.set_ylim(ylim)
+        if not show_axis:
+            self.ax.set_axis_off()
+        self.goal_handle = self.ax.plot(*goal, color=goal_color, marker=goal_marker, markersize=goal_markersize)[0]
+        self.robot_handles, _ = robot.init_plot(ax=self.ax, color=robot_color, markersize=robot_markersize, alpha=robot_alpha)
+        self.obstacle_handles = []
+        for o in obstacles:
+            lh, _ = o.init_plot(ax=self.ax, fc=obstacle_color, ec=obstacle_edge_color, show_name=show_obs_name,
+                                show_reference=False)
+            self.obstacle_handles.append(lh)
+        self.travelled_path_handle = self.ax.plot([], [], color=travelled_path_color, linestyle=travelled_path_linestyle, linewidth=travelled_path_linewidth, zorder=0)[0]
+        # Simulation ctrl
+        self.fig_open = True
+        self.paused = True
+        self.step_once = False
+        self.fig.canvas.mpl_connect('close_event', self.on_close)
+        self.fig.canvas.mpl_connect('key_press_event', self.on_press)
+        # Memory
+        self.travelled_path = []
+        #
+        # self.update(init_robot_state, goal, 0)
+
+    def update(self, robot_state=None, goal=None, time=None):
+        if time:
+            self.ax.set_title("Time: {:.1f} s".format(time))
+
+        # Obstacles
+        [oh[0].update_plot(oh[1]) for oh in zip(self.obstacles, self.obstacle_handles)]
+
+        # Robot and goal position
+        if robot_state is not None:
+            self.travelled_path += list(self.robot.h(robot_state))
+            self.robot.update_plot(robot_state, self.robot_handles)
+            self.travelled_path_handle.set_data(self.travelled_path[::2], self.travelled_path[1::2])
+        if goal is not None:
+            self.goal_handle.set_data(*goal)
+
+
+    def on_close(self, event):
+        self.fig_open = False
+
+    def on_press(self, event):
+        if event.key == ' ':
+            self.paused = not self.paused
+        elif event.key == 'right':
+            self.step_once = True
+            self.paused = True
+        else:
+            print(event.key)
+
+
+class SoadsGUI(SceneGUI):
+    def __init__(self, robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis=False,
+                 robot_color='k', robot_markersize=14, robot_alpha=0.7,
+                 obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False,
+                 goal_color='y', goal_marker='*', goal_markersize=16,
+                 travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2,
+                 obstacles_star_color='r', obstacles_star_show_reference=True, obstacles_star_alpha=0.1):
+        self.obstacle_star_handles = []
+        self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference,
+                                            'alpha': obstacles_star_alpha, 'zorder': 0}
+        super().__init__(robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis, robot_color, robot_markersize,
+                         robot_alpha, obstacle_color, obstacle_edge_color, show_obs_name, goal_color, goal_marker,
+                         goal_markersize, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth)
+
+    def update(self, robot_state=None, goal=None, time=None, obstacles_star=None):
+        # SceneFig update
+        super().update(robot_state, goal, time)
+        # Star obstacles
+        [h.remove() for h in self.obstacle_star_handles if h is not None]
+        self.obstacle_star_handles = []
+        if obstacles_star is not None:
+            for o in obstacles_star:
+                lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options)
+                self.obstacle_star_handles += lh
+
+class TunnelMPCGUI(SceneGUI):
+
+    def __init__(self, robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis=False,
+                 robot_color='y', robot_markersize=14, robot_alpha=0.7,
+                 obstacle_color='lightgrey', obstacle_edge_color='k', show_obs_name=False,
+                 goal_color='g', goal_marker='*', goal_markersize=16,
+                 travelled_path_color='k', travelled_path_linestyle='-', travelled_path_linewidth=2,
+                 target_path_color='g', target_path_linestyle='-', target_path_linewidth=4, target_path_marker=None,
+                 mpc_path_color='k', mpc_path_linestyle=':', mpc_path_linewidth=4,
+                 mpc_tunnel_color='r', mpc_tunnel_linestyle='--', mpc_tunnel_linewidth=2,
+                 obstacles_star_color='r', obstacles_star_show_reference=False, obstacles_star_alpha=0.1,
+                 mpc_horizon=None, rho0=None):
+        self.obstacle_star_handles = []
+        self.obstacles_star_draw_options = {'fc': obstacles_star_color, 'show_reference': obstacles_star_show_reference,
+                                            'alpha': obstacles_star_alpha, 'zorder': 0}
+        super().__init__(robot, obstacles, init_robot_state, goal, xlim, ylim, show_axis, robot_color, robot_markersize,
+                         robot_alpha, obstacle_color, obstacle_edge_color, show_obs_name, goal_color, goal_marker,
+                         goal_markersize, travelled_path_color, travelled_path_linestyle, travelled_path_linewidth)
+        self.target_path_handle = self.ax.plot([], [], color=target_path_color, linestyle=target_path_linestyle, linewidth=target_path_linewidth, marker=target_path_marker, zorder=0)[0]
+        self.mpc_path_handle = self.ax.plot([], [], color=mpc_path_color, linestyle=mpc_path_linestyle, linewidth=mpc_path_linewidth, zorder=0, dashes=(0.8, 0.8))[0]
+        self.mpc_tunnel_handle = self.ax.plot([], [], color=mpc_tunnel_color, linestyle=mpc_tunnel_linestyle, linewidth=mpc_tunnel_linewidth, zorder=0)[0]
+
+        self.fig_mpc_solution = None
+        if mpc_horizon:
+            self.fig_mpc_solution, self.ax_mpc_solution = plt.subplots(2, 2)
+            self.s_handle = self.ax_mpc_solution[0, 0].plot(np.linspace(1, mpc_horizon, mpc_horizon), [None] * mpc_horizon, '-o')[0]
+            self.skappa_handle = self.ax_mpc_solution[0, 0].plot([0, mpc_horizon], [None, None], 'r--')[0]
+            self.ax_mpc_solution[0, 0].plot([0, mpc_horizon], [mpc_horizon, mpc_horizon], 'r--')
+            self.ax_mpc_solution[0, 0].set_ylim(0, 1.1*mpc_horizon)
+            self.rho_handle = self.ax_mpc_solution[1, 0].plot([0, mpc_horizon], [rho0, rho0], 'k--')[0]
+            self.emax_handle = self.ax_mpc_solution[1, 0].plot([0, mpc_horizon], [None, None], 'r--')[0]
+            self.e_handle = self.ax_mpc_solution[1, 0].plot(np.linspace(0, mpc_horizon, mpc_horizon+1, '-o'), [None] * (mpc_horizon+1), '-o')[0]
+            self.ax_mpc_solution[1, 0].set_ylim(0, 1.1*rho0)
+            # Assumes 2 control signals
+            self.u1_handle = self.ax_mpc_solution[0, 1].plot(np.linspace(0, mpc_horizon, mpc_horizon+1), [None] * (mpc_horizon + 1), '-o')[0]
+            self.ax_mpc_solution[0, 1].plot([0, mpc_horizon], [robot.u_min[0], robot.u_min[0]], 'r--')
+            self.ax_mpc_solution[0, 1].plot([0, mpc_horizon], [robot.u_max[0], robot.u_max[0]], 'r--')
+            self.u2_handle = self.ax_mpc_solution[1, 1].plot(np.linspace(0, mpc_horizon, mpc_horizon+1), [None] * (mpc_horizon + 1), '-o')[0]
+            self.ax_mpc_solution[1, 1].plot([0, mpc_horizon], [robot.u_min[1], robot.u_min[1]], 'r--')
+            self.ax_mpc_solution[1, 1].plot([0, mpc_horizon], [robot.u_max[1], robot.u_max[1]], 'r--')
+
+        self.update(init_robot_state, goal, 0)
+
+    def update(self, robot_state=None, goal=None, time=None, obstacles_star=None, target_path=None, mpc_path=None,
+               s=None, u=None, e=None, rho=None, e_max=None, s_kappa=None, timing=None):
+        # SceneFig update
+        super().update(robot_state, goal, time)
+
+        if timing:
+            if time:
+                self.ax.set_title("Time: {:.1f} s\nCompute Time ({:.1f} / {:.1f} / {:.1f})".format(time, timing['workspace'], timing['target'], timing['mpc']))
+            else:
+                self.ax.set_title("Compute Time ({:.1f} / {:.1f} / {:.1f})".format(timing['workspace'], timing['target'], timing['mpc']))
+
+        # Star obstacles
+        [h.remove() for h in self.obstacle_star_handles if h is not None]
+        self.obstacle_star_handles = []
+        if obstacles_star is not None:
+            for o in obstacles_star:
+                lh, _ = o.draw(ax=self.ax, **self.obstacles_star_draw_options)
+                self.obstacle_star_handles += lh
+
+        # Trajectories
+        if mpc_path is not None:
+            self.mpc_path_handle.set_data(mpc_path[::2], mpc_path[1::2])
+        if target_path is not None:
+            self.target_path_handle.set_data(target_path[::2], target_path[1::2])
+            if e_max is not None:
+                tunnel_polygon = shapely.geometry.LineString(list(zip(target_path[::2], target_path[1::2]))).buffer(
+                    e_max)
+                if tunnel_polygon.geom_type == 'Polygon':
+                    self.mpc_tunnel_handle.set_data(*tunnel_polygon.exterior.xy)
+                else:
+                    print("[SceneFig]: Tunnel polygon not polygon.")
+                    print(tunnel_polygon)
+
+        # MPC solution plot
+        if self.fig_mpc_solution is not None:
+            if s is not None:
+                self.s_handle.set_ydata(s[1:])
+            if s_kappa is not None:
+                self.skappa_handle.set_ydata([s_kappa, s_kappa])
+            if e is not None:
+                self.e_handle.set_ydata(e)
+            if e_max is not None:
+                self.emax_handle.set_ydata([e_max, e_max])
+            if rho is not None:
+                self.rho_handle.set_ydata([rho, rho])
+            if u is not None:
+                self.u1_handle.set_ydata(u[::2])
+                self.u2_handle.set_ydata(u[1::2])
-- 
GitLab