Merge Rabbit demo to v2 beta.

.gitignore

@@ -48,3 +48,166 @@ $RECYCLE.BIN/
 Network Trash Folder
 Temporary Items
 .apdisk
+
+docs/_build/
+
+.vscode/*
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/

demos/cart_pole/bo_loss_func.m

@@ -0,0 +1,18 @@
+function loss = bo_loss_func(y, x)
+    loss1 = -y^2;
+    loss2 = -(x(3)-pi)^2;
+    loss3 = -x(4)^2;
+    loss4 = -x(2)^2;
+    % fprintf("Loss: y_error: %.4f \t theta: %.4f \t dtheta: %.4f \t dx: %.4f \n", [loss1, loss2, loss3, loss4]);
+    % c1 = 1000;
+    c1 = 0;
+    % c2 = 1000;
+    % c2 = 200;
+    c2 = 100;
+    c3 = 1;
+    % c4 = 100;
+    c4 = 0;
+    fprintf("Loss: y_error: %.4f \t theta: %.4f \t dtheta: %.4f \t dx: %.4f \n", [c1 * loss1, c2 * loss2, c3 *  loss3, c4 * loss4]);
+    loss = c1 * loss1 + c2 * loss2 + c3 * loss3 + c4 * loss4;
+
+end

demos/cart_pole/clip_theta.m

@@ -0,0 +1,10 @@
+function x_clipped = clip_theta(x)
+    x_clipped = x;
+    theta = x(3);
+    r = mod(theta, 2*pi);
+    if r > pi
+        x_clipped(3) = r - 2*pi;
+    else
+        x_clipped(3) = r;
+    end
+end

demos/cart_pole/init_settings_cart_pole.m

@@ -0,0 +1,52 @@
+clear all;
+close all;
+dt = 0.01;
+
+%% Direct CLF without FL works for these parameters.
+% params.l = 1.0;    % [m]        length of pendulum
+% params.m = 1.0;    % [kg]       mass of pendulum
+% params.M = 1.0;    % [kg]       mass of cart
+% params.g = 9.81;   % [m/s^2]    acceleration of gravity
+% params.b = 0.1;    % [s*Nm/rad] friction coefficient
+
+params.l = 0.5;    % [m]        length of pendulum
+params.m = 0.5;    % [kg]       mass of pendulum
+params.M = 2.0;    % [kg]       mass of cart
+params.g = 9.81;   % [m/s^2]    acceleration of gravity
+params.b = 0.1;    % [s*Nm/rad] friction coefficient
+
+params.cbf.rate = 1;
+
+u_bound = 50;
+params.u_max = u_bound;
+params.u_min = -u_bound;
+
+params.ratio_u_diff = 0;
+params.weight.slack = 1000;
+%params.weight.input = eye(params.udim); % tune this to allocate different
+%weight in each u coordinate
+params.clf.rate = 1;
+
+%% Feedback linearization specific params.
+params.bezier = [-0.0630040307234950, ...
+    -2.94395202983077, ...
+    4.23655180750478, ...
+    -10.8954633332227, ...
+    13.3619387476085, ...
+    -13.4630778220299, ...
+    9.93449615595792, ...
+    -3.07150993759866, ...
+    3.86268687914481, ...
+    2.72221821558065, ...
+    3.15006153603993];
+params.phase_max = 1;
+params.phase_min = 0;
+params.epsilon_FL = 0.015;
+params.Kp_FL = 0.99;
+params.Kd_FL = 2.0;
+
+control_sys = CartPole(params);
+
+plant_sys = CartPole(params);
+
+settings.dt = dt;

demos/cart_pole/main_cart_pole.m

@@ -0,0 +1,21 @@
+init_settings_cart_pole;
+verbose = 0;
+
+with_slack = 1;
+% initial state
+% x0 = [0; 0; pi - 0.1; 0;];
+x0 = [0; 0; 0.1; 0;];
+% simulation time
+sim_t = 3;
+
+
+[xs, us, ts, Vs, ~, ~, ~, ~] = rollout_lyapunov_controller( ...
+    x0, plant_sys, control_sys, @control_sys.ctrlClfQp, dt, sim_t, with_slack, [], verbose);
+results.vanilla.trajectory = xs;
+results.vanilla.controls = us;
+results.vanilla.stamps = ts;
+
+draw_rollout(results.vanilla, 0, dt, plant_sys, control_sys, "Vanilla")
+
+figure;
+plot(ts, Vs);