From 7d0a27ac7bf17fbb9917f0be18093a9d0371fbcb Mon Sep 17 00:00:00 2001
From: scalinon <sylvain.calinon@idiap.ch>
Date: Sun, 29 Dec 2019 15:33:44 +0100
Subject: [PATCH] ergodic control examples added
---
CMakeLists.txt | 21 +-
README.md | 201 ++++++--
examples.md | 185 -------
src/demo_ergodicControl_2D01.cpp | 586 +++++++++++++++++++++
src/demo_ergodicControl_nD01.cpp | 850 +++++++++++++++++++++++++++++++
5 files changed, 1624 insertions(+), 219 deletions(-)
delete mode 100644 examples.md
create mode 100644 src/demo_ergodicControl_2D01.cpp
create mode 100644 src/demo_ergodicControl_nD01.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c2c3ee5..9cb27be 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,6 +95,25 @@ set(GFX2_SUPPORT_SOURCES
# Executables
+add_executable(demo_ergodicControl_2D01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_ergodicControl_2D01.cpp
+ ${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
+)
+target_link_libraries(demo_ergodicControl_2D01
+ ${LIBRARIES}
+)
+
+add_executable(demo_ergodicControl_nD01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_ergodicControl_nD01.cpp
+ ${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
+ ${PROJECT_SOURCE_DIR}/src/utils/ImGuizmo.cpp
+)
+target_link_libraries(demo_ergodicControl_nD01
+ ${LIBRARIES}
+)
+
add_executable(demo_GMR01
${GFX2_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_GMR01.cpp
@@ -397,4 +416,4 @@ target_link_libraries(demo_TPGMR01
${GLEW_LIBRARIES}
${GLFW_LIB}
${ARMADILLO_LIBRARIES}
- )
\ No newline at end of file
+ )
diff --git a/README.md b/README.md
index e254947..6b5eef9 100644
--- a/README.md
+++ b/README.md
@@ -1,18 +1,121 @@
-# PbDlib-cpp
-
-PbDlib-cpp is a set of tools in C++ combining statistical learning, optimal control and differential geometry for programming-by-demonstration applications.
-Other versions of the library are available at http://www.idiap.ch/software/pbdlib/ (in Matlab, C++ and Python).
-
-Contributors: Philip Abbet, Sylvain Calinon, Daniel Berio, Ioannis Havoutis, Ajay Tanwani, Emmanuel Pignat, Noemie Jaquier, Andras Kupcsik
-
-### Support
-
-This work was in part supported by the [DexROV project](http://www.dexrov.eu/) through the European Commission H2020 programme (Grant \#635491).
-
-
-### Prerequisite
-
-The program requires:
+# pbdlib-cpp
+
+<p>PbDlib is a collection of C++ source codes for robot programming by demonstration (learning from demonstration). It includes various functionalities at the crossroad of statistical learning, dynamical systems, optimal control and Riemannian geometry.</p>
+<p>PbDlib can be used in applications requiring task adaptation, human-robot skill transfer, safe controllers based on minimal intervention principle, as well as for probabilistic motion analysis and synthesis in multiple coordinate systems.</p>
+<p>Other versions of the library in Matlab (compatible with GNU Octave) and Python are also available at http://www.idiap.ch/software/pbdlib/ (currently, the Matlab version has the most functionalities).</p>
+
+
+### Usage
+
+This project builds a number of executables in the `bin` folder, with th corresponding C++ source codes listed in the table below. The corresponding publications are also listed below.
+
+
+### List of examples
+
+| Filename | ref. | Description |
+|----------|------|-------------|
+| [demo_ergodicControl_2D01.cpp](./src/demo_ergodicControl_2D01.cpp) | [[5]](#ref-5) | 2D ergodic control with spectral multiscale coverage (SMC) algorithm |
+| [demo_GMR01.cpp](./src/demo_GMR01.cpp) | [[5]](#ref-5) | Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction |
+| [demo_GPR01.cpp](./src/demo_GPR01.cpp) | [[2]](#ref-2) | Gaussian process regression (GPR) |
+| [demo_HSMM_batchLQR01.cpp](./src/demo_HSMM_batchLQR01.cpp) | [[2]](#ref-2) | Use of HSMM (with lognormal duration model) and batch LQR (with position only) for motion synthesis |
+| [demo_infHorLQR01.cpp](./src/demo_infHorLQR01.cpp) | [[2]](#ref-2) | Discrete infinite horizon linear quadratic regulation |
+| [demo_LWR_batch01.cpp](./src/demo_LWR_batch01.cpp) | [[2]](#ref-2) | Locally weighted regression (LWR) with radial basis functions (RBF), using batch computation |
+| [demo_LWR_iterative01.cpp](./src/demo_LWR_iterative01.cpp) | [[2]](#ref-2) | Locally weighted regression (LWR) with radial basis functions (RBF), using iterative computation |
+| [demo_MPC_batch01.cpp](./src/demo_MPC_batch01.cpp) | [[2]](#ref-2), [[7]](#ref-7) | Model predictive control (MPC) with batch linear quadratic tracking (LQT) formulation |
+| [demo_MPC_iterative01.cpp](./src/demo_MPC_iterative01.cpp) | [[2]](#ref-2), [[7]](#ref-7) | Model predictive control (MPC) with iterative linear quadratic tracking (LQT) formulation |
+| [demo_MPC_semitied01.cpp](./src/demo_MPC_semitied01.cpp) | [[2]](#ref-2), [[7]](#ref-7) | MPC with semi-tied covariances |
+| [demo_MPC_velocity01.cpp](./src/demo_MPC_velocity01.cpp) | [[2]](#ref-2), [[7]](#ref-7) | MPC with an objective including velocity tracking |
+| [demo_online_GMM01.cpp](./src/demo_online_GMM01.cpp) | [[2]](#ref-2), [[6]](#ref-6) | Online GMM learning and LQR-based trajectory generation |
+| [demo_online_HSMM01.cpp](./src/demo_online_HSMM01.cpp) | [[2]](#ref-2), [[6]](#ref-6) | Online HSMM learning and sampling with LQR-based trajectory generation |
+| [demo_proMP01.cpp](./src/demo_proMP01.cpp) | [[5]](#ref-5) | Conditioning on trajectory distributions with ProMP |
+| [demo_Riemannian_cov_GMR01.cpp](./src/demo_Riemannian_cov_GMR01.cpp) | [[4]](#ref-4) | GMR with time as input and covariance data as output by relying on Riemannian manifold |
+| [demo_Riemannian_cov_interp02.cpp](./src/demo_Riemannian_cov_interp02.cpp) | [[4]](#ref-4) | Covariance interpolation on Riemannian manifold from a GMM with augmented covariances |
+| [demo_Riemannian_pose_batchLQR01.cpp](./src/demo_Riemannian_pose_batchLQR01.cpp) | [[3]](#ref-3) | Linear quadratic regulation of pose by relying on Riemannian manifold and batch LQR (MPC without constraints) |
+| [demo_Riemannian_pose_GMM01.cpp](./src/demo_Riemannian_pose_GMM01.cpp) | [[3]](#ref-3) | GMM for 3D position and unit quaternion data by relying on Riemannian manifold |
+| [demo_Riemannian_pose_infHorLQR01.cpp](./src/demo_Riemannian_pose_infHorLQR01.cpp) | [[3]](#ref-3) | Linear quadratic regulation of pose by relying on Riemannian manifold and infinite-horizon LQR |
+| [demo_Riemannian_pose_TPGMM01.cpp](./src/demo_Riemannian_pose_TPGMM01.cpp) | [[3]](#ref-3) | TP-GMM of pose (R3 x S3) with two frames |
+| [demo_Riemannian_quat_infHorLQR01.cpp](./src/demo_Riemannian_quat_infHorLQR01.cpp) | [[3]](#ref-3) | Linear quadratic regulation on hypersphere (orientation as unit quaternions) by relying on Riemannian manifold and infinite-horizon LQR |
+| [demo_Riemannian_quat_TPGMM01.cpp](./src/demo_Riemannian_quat_TPGMM01.cpp) | [[3]](#ref-3) | TP-GMM on S3 (unit quaternion) with two frames |
+| [demo_Riemannian_sphere_GMM01.cpp](./src/demo_Riemannian_sphere_GMM01.cpp) | [[3]](#ref-3) | GMM for data on a sphere by relying on Riemannian manifold |
+| [demo_Riemannian_sphere_infHorLQR01.cpp](./src/demo_Riemannian_sphere_infHorLQR01.cpp) | [[3]](#ref-3) | Linear quadratic regulation on a sphere by relying on Riemannian manifold and infinite-horizon LQR |
+| [demo_Riemannian_sphere_product01.cpp](./src/demo_Riemannian_sphere_product01.cpp) | [[3]](#ref-3) | Gaussian product on sphere |
+| [demo_Riemannian_sphere_TPGMM01.cpp](./src/demo_Riemannian_sphere_TPGMM01.cpp) | [[3]](#ref-3) | TP-GMM for data on a sphere by relying on Riemannian manifold (with single frame) |
+| [demo_TPbatchLQR01.cpp](./src/demo_TPbatchLQR01.cpp) | [[1]](#ref-1) | Linear quadratic control (unconstrained linear MPC) acting in multiple frames, which is equivalent to the fusion of Gaussian controllers |
+| [demo_TPGMMProduct01.cpp](./src/demo_TPGMMProduct01.cpp) | [[1]](#ref-1) | Product of Gaussians for a Task-Parametrized GMM |
+| [demo_TPGMR01.cpp](./src/demo_TPGMR01.cpp) | [[1]](#ref-1) | Task-Parametrized GMM with GMR (time as input), the model is able to adapt to continuously changing task parameters. |
+
+
+### References
+
+If you find PbDLib useful for your research, please cite the related publications!
+
+<p><a name="ref-1">
+[1] Calinon, S. (2016). <strong>A Tutorial on Task-Parameterized Movement Learning and Retrieval</strong>. Intelligent Service Robotics (Springer), 9:1, 1-29.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Calinon-JIST2015.pdf)
+[[bib]](http://calinon.ch/papers/Calinon-JIST2015.bib)
+<br><strong>(Ref. for GMM, TP-GMM, MFA, MPPCA, GPR, trajGMM)</strong>
+</p>
+
+<p><a name="ref-2">
+[2] Calinon, S. and Lee, D. (2019). <strong>Learning Control</strong>. Vadakkepat, P. and Goswami, A. (eds.). Humanoid Robotics: a Reference, pp. 1261-1312. Springer.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Calinon-Lee-learningControl.pdf)
+[[bib]](http://calinon.ch/papers/Calinon-Lee-learningControl.bib)
+<br><strong>(Ref. for MPC, LQR, HMM, HSMM)</strong>
+</p>
+
+<p><a name="ref-3">
+[3] Calinon, S. and Jaquier, N. (2019). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. arXiv:1909.05946.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Calinon-arXiv2019.pdf)
+[[bib]](http://calinon.ch/papers/Calinon-arXiv2019.bib)
+<br><strong>(Ref. for Riemannian manifolds)</strong>
+</p>
+
+<p><a name="ref-4">
+[4] Jaquier, N. and Calinon, S. (2017). <strong>Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with sEMG</strong>. In Proc. of the IEEE/RSJ Intl Conf. on Intelligent Robots and Systems (IROS), pp. 59-64.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Jaquier-IROS2017.pdf)
+[[bib]](http://calinon.ch/papers/Jaquier-IROS2017.bib)
+<br><strong>(Ref. for S^+ Riemannian manifolds)</strong>
+</p>
+
+<p><a name="ref-5">
+[5] Calinon, S. (2019). <strong>Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series</strong>. Bouguila, N. and Fan, W. (eds). Mixture Models and Applications, pp. 39-57. Springer.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Calinon_MMchapter2019.pdf)
+[[bib]](http://calinon.ch/papers/Calinon_MMchapter2019.bib)
+<br><strong>(Ref. for ergodic control, Bezier curves, LWR, GMR, probabilistic movement primitives)</strong>
+</p>
+
+<p><a name="ref-6">
+[6] Bruno, D., Calinon, S. and Caldwell, D.G. (2017). <strong>Learning Autonomous Behaviours for the Body of a Flexible Surgical Robot</strong>. Autonomous Robots, 41:2, 333-347.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Bruno-AURO2017.pdf)
+[[bib]](http://calinon.ch/papers/Bruno-AURO2017.bib)
+<br><strong>(Ref. for DP-means)</strong>
+</p>
+
+<p><a name="ref-7">
+[7] Berio, D., Calinon, S. and Fol Leymarie, F. (2017). <strong>Generating Calligraphic Trajectories with Model Predictive Control</strong>. In Proc. of the 43rd Conf. on Graphics Interface, pp. 132-139.
+</a><br>
+[[pdf]](http://calinon.ch/papers/Berio-GI2017.pdf)
+[[bib]](http://calinon.ch/papers/Berio-GI2017.bib)
+<br><strong>(Ref. for Bezier curves as MPC with viapoints)</strong>
+</p>
+
+<p><a name="ref-8">
+[8] EPFL EE613 course "Machine Learning for Engineers"
+</a><br>
+[[url]](http://calinon.ch/teaching.htm)
+<br><strong>(Ref. for machine learning teaching material)</strong>
+</p>
+
+
+### Installation prerequisite
+
+PbDlib requires:
- *glfw3* (lightweight and portable library for managing OpenGL contexts, windows and inputs)
- *GLEW* (The OpenGL Extension Wrangler Library)
@@ -21,14 +124,14 @@ The program requires:
Instructions are given below.
-ImGui (graphical user interface library for C++, [https://github.com/ocornut/imgui](https://github.com/ocornut/imgui)) and
-gfx_ui (a minimal geometry editing UI, [https://github.com/colormotor/gfx\_ui](https://github.com/colormotor/gfx_ui)), are also
-used and provided as part of this package. They are both distributed under the MIT license (see the docs/ folder).
+*ImGui* (graphical user interface library for C++, [https://github.com/ocornut/imgui](https://github.com/ocornut/imgui)) and
+*gfx_ui* (a minimal geometry editing UI, [https://github.com/colormotor/gfx\_ui](https://github.com/colormotor/gfx_ui)), are also
+used and provided as part of this package. They are both distributed under the *MIT license* (see the docs/ folder).
-#### glfw3 installation
+### Dependencies installation on Debian and Ubuntu
-**On Debian and Ubuntu:**
+#### glfw3
```
sudo apt-get install libglfw3-dev
@@ -52,31 +155,25 @@ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
*Note:* The `-DBUILD_SHARED_LIBS` is necessary otherwise cmake will create a
static library.
-
-#### GLEW installation
-
-**On Debian and Ubuntu:**
+#### GLEW
```
sudo apt-get install libglew-dev
```
-
-#### LAPACK installation
-
-**On Debian and Ubuntu:**
+#### LAPACK
```
sudo apt-get install liblapack-dev
```
-
-#### Armadillo installation
+#### Armadillo
See [http://arma.sourceforge.net/download.html](http://arma.sourceforge.net/download.html)
for instructions.
-### Compilation
+
+### Compilation (on Linux and MacOS)
```
cd pbdlib-cpp
@@ -86,13 +183,39 @@ cmake ..
make
```
-### References and list of examples
-See [examples.md](./examples.md)
+### Compilation (on Windows)
+
+First, download all the necessary dependencies. They are (but Armadillo) provided as
+a **binary package** on their own homepage. We recommend to download and extract them
+all in the same folder.
+
+**glfw3**: https://www.glfw.org/download.html
+
+**GLEW**: http://glew.sourceforge.net/index.html
+
+**Armadillo**: http://arma.sourceforge.net/download.html
+
+Then use cmake-gui (http://cmake.org) to generate a solution for you version of Visual
+Studio. You'll need to manually indicate the path to the following dependencies when
+requested:
+
+ * `ARMADILLO_DIR`: Path to Armadillo
+ * `GLEW_DIR`: Path to GLEW
+ * `GLFW_DIR`: Path to GLFW
+
+Once the solution has been generated, open it (`pbdlib_gui.sln` in your build folder)
+and compile as usual.
### Gallery
+[demo\_demo\_ergodicControl\_2D01.cpp](./src/demo_ergodicControl_2D01.cpp)
+
+
+
+***
+
[demo\_GMR01.cpp](./src/demo_GMR01.cpp)
@@ -234,3 +357,15 @@ See [examples.md](./examples.md)
[demo\_TPGMR01.cpp](./src/demo_TPGMR01.cpp)
+
+
+### License
+
+Copyright (c) 2015-2019 Idiap Research Institute, http://idiap.ch/
+
+PbDlib is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License version 3 as published by the Free Software Foundation.
+
+PbDlib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
diff --git a/examples.md b/examples.md
deleted file mode 100644
index b4da0a9..0000000
--- a/examples.md
+++ /dev/null
@@ -1,185 +0,0 @@
-### References
-
-Did you find PbDlib useful for your research? Please acknowledge the authors in any academic publications that used parts of these codes.
-<br><br>
-
-#### Ref. [1]
-**Tutorial (GMM, TP-GMM, MFA, MPPCA, GMR, LWR, GPR, MPC, LQR, trajGMM)**<br>
-[Link to publication](http://calinon.ch/papers/Calinon-JIST2015.pdf)
-```
-@article{Calinon16JIST,
- author="Calinon, S.",
- title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
- journal="Intelligent Service Robotics",
- publisher="Springer Berlin Heidelberg",
- year="2016",
- volume="9",
- number="1",
- pages="1--29",
- doi="10.1007/s11370-015-0187-9",
-}
-```
-
-#### Ref. [2]
-**HMM, HSMM**<br>
-[Link to publication](http://calinon.ch/papers/Rozo-Frontiers2016.pdf)
-```
-@article{Rozo16Frontiers,
- author="Rozo, L. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
- title="Learning Controllers for Reactive and Proactive Behaviors in Human-Robot Collaboration",
- journal="Frontiers in Robotics and {AI}",
- year="2016",
- month="June",
- volume="3",
- number="30",
- pages="1--11",
- doi="10.3389/frobt.2016.00030"
-}
-```
-
-#### Ref. [3]
-**Riemannian manifolds (S2,S3)**<br>
-[Link to publication](http://calinon.ch/papers/Zeestraten-RAL2017.pdf)
-```
-@article{Zeestraten17RAL,
- author="Zeestraten, M. J. A. and Havoutis, I. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
- title="An Approach for Imitation Learning on {R}iemannian Manifolds",
- journal="{IEEE} Robotics and Automation Letters ({RA-L})",
- year="2017",
- month="June",
- volume="2",
- number="3",
- pages="1240--1247"
- doi="10.1109/LRA.2017.2657001",
-}
-```
-
-#### Ref. [4]
-**Riemannian manifolds (S+)**<br>
-[Link to publication](http://calinon.ch/papers/Jaquier-IROS2017.pdf)
-```
-@inproceedings{Jaquier17IROS,
- author="Jaquier, N. and Calinon, S.",
- title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with {sEMG}",
- booktitle="Proc. {IEEE/RSJ} Intl Conf. on Intelligent Robots and Systems ({IROS})",
- year="2017",
- month="September",
- address="Vancouver, Canada",
- pages=""
-}
-```
-
-#### Ref. [5]
-**Semi-tied GMM**<br>
-[Link to publication](http://calinon.ch/papers/Tanwani-RAL2016.pdf)
-```
-@article{Tanwani16RAL,
- author="Tanwani, A. K. and Calinon, S.",
- title="Learning Robot Manipulation Tasks with Task-Parameterized Semi-Tied Hidden Semi-{M}arkov Model",
- journal="{IEEE} Robotics and Automation Letters ({RA-L})",
- year="2016",
- month="January",
- volume="1",
- number="1",
- pages="235--242",
- doi="10.1109/LRA.2016.2517825"
-}
-```
-
-#### Ref. [6]
-**DP-means**<br>
-[Link to publication](http://calinon.ch/papers/Bruno-AURO2017.pdf)
-```
-@article{Bruno17AURO,
- author="Bruno, D. and Calinon, S. and Caldwell, D. G.",
- title="Learning Autonomous Behaviours for the Body of a Flexible Surgical Robot",
- journal="Autonomous Robots",
- year="2017",
- month="February",
- volume="41",
- number="2",
- pages="333--347",
- doi="10.1007/s10514-016-9544-6"
-}
-```
-
-#### Ref. [7]
-**Shared control, adaptive teleoperation**<br>
-```
-@inproceedings{Havoutis17ICRA,
- author="Havoutis, I. and Calinon, S.",
- title="Supervisory teleoperation with online learning and optimal control",
- booktitle=ICRA,
- year="2017",
- month="May-June",
- address="Singapore",
- pages="1534--1540"
-}
-```
-
-#### Ref. [8]
-**Keypoint-based motion through MPC**<br>
-[Link to publication](http://calinon.ch/papers/Berio-GI2017.pdf)
-```
-@inproceedings{Berio17GI,
- author="Berio, D. and Calinon, S. and Fol Leymarie, F.",
- title="Generating Calligraphic Trajectories with Model Predictive Control",
- booktitle="Proc. 43rd Conf. on Graphics Interface",
- year="2017",
- month="May",
- address="Edmonton, AL, Canada",
- pages="132--139",
- doi="10.20380/GI2017.17"
-}
-```
-
-#### Ref. [9]
-**Adaptive assistance**<br>
-```
-@article{Pignat17RAS,
- author="Pignat, E. and Calinon, S.",
- title="Learning adaptive dressing assistance from human demonstration",
- journal="Robotics and Autonomous Systems",
- year="2017",
- month="July",
- volume="93",
- number="",
- pages="61--75",
- doi="10.1016/j.robot.2017.03.017",
-}
-```
-
-
-### List of examples
-
-This project will build a number of executables, as listed in the table below.
-
-| Filename | ref. | Description |
-|----------|------|-------------|
-| [demo_GMR01.cpp](./src/demo_GMR01.cpp) | [[1]](#ref-1) | Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction |
-| [demo_GPR01.cpp](./src/demo_GPR01.cpp) | [[1]](#ref-1) | Gaussian process regression (GPR) |
-| [demo_HSMM_batchLQR01.cpp](./src/demo_HSMM_batchLQR01.cpp) | [[2]](#ref-2) | Use of HSMM (with lognormal duration model) and batch LQR (with position only) for motion synthesis |
-| [demo_LWR_batch01.cpp](./src/demo_LWR_batch01.cpp) | [[1]](#ref-1) | Locally weighted regression (LWR) with radial basis functions (RBF), using batch computation |
-| [demo_LWR_iterative01.cpp](./src/demo_LWR_iterative01.cpp) | [[1]](#ref-1) | Locally weighted regression (LWR) with radial basis functions (RBF), using iterative computation |
-| [demo_MPC_batch01.cpp](./src/demo_MPC_batch01.cpp) | [[1]](#ref-1), [[8]](#ref-8) | Model predictive control (MPC) with batch linear quadratic tracking (LQT) formulation |
-| [demo_MPC_iterative01.cpp](./src/demo_MPC_iterative01.cpp) | [[1]](#ref-1), [[8]](#ref-8) | Model predictive control (MPC) with iterative linear quadratic tracking (LQT) formulation |
-| [demo_MPC_semitied01.cpp](./src/demo_MPC_semitied01.cpp) | [[5]](#ref-5), [[8]](#ref-8) | MPC with semi-tied covariances |
-| [demo_MPC_velocity01.cpp](./src/demo_MPC_velocity01.cpp) | [[1]](#ref-1), [[8]](#ref-8) | MPC with an objective including velocity tracking |
-| [demo_online_GMM01.cpp](./src/demo_online_GMM01.cpp) | [[6]](#ref-6) | Online GMM learning and LQR-based trajectory generation |
-| [demo_online_HSMM01.cpp](./src/demo_online_HSMM01.cpp) | [[2]](#ref-2), [[7]](#ref-7) | Online HSMM learning and sampling with LQR-based trajectory generation |
-| [demo_proMP01.cpp](./src/demo_proMP01.cpp) | [] | Conditioning on trajectory distributions with ProMP |
-| [demo_Riemannian_cov_GMR01.cpp](./src/demo_Riemannian_cov_GMR01.cpp) | [[4]](#ref-4) | GMR with time as input and covariance data as output by relying on Riemannian manifold |
-| [demo_Riemannian_cov_interp02.cpp](./src/demo_Riemannian_cov_interp02.cpp) | [[4]](#ref-4) | Covariance interpolation on Riemannian manifold from a GMM with augmented covariances |
-| [demo_Riemannian_pose_batchLQR01.cpp](./src/demo_Riemannian_pose_batchLQR01.cpp) | [] | Linear quadratic regulation of pose by relying on Riemannian manifold and batch LQR (MPC without constraints) |
-| [demo_Riemannian_pose_GMM01.cpp](./src/demo_Riemannian_pose_GMM01.cpp) | [] | GMM for 3D position and unit quaternion data by relying on Riemannian manifold |
-| [demo_Riemannian_pose_infHorLQR01.cpp](./src/demo_Riemannian_pose_infHorLQR01.cpp) | [] | Linear quadratic regulation of pose by relying on Riemannian manifold and infinite-horizon LQR |
-| [demo_Riemannian_pose_TPGMM01.cpp](./src/demo_Riemannian_pose_TPGMM01.cpp) | [] | TP-GMM of pose (R3 x S3) with two frames |
-| [demo_Riemannian_quat_infHorLQR01.cpp](./src/demo_Riemannian_quat_infHorLQR01.cpp) | [[3]](#ref-3) | Linear quadratic regulation on hypersphere (orientation as unit quaternions) by relying on Riemannian manifold and infinite-horizon LQR |
-| [demo_Riemannian_quat_TPGMM01.cpp](./src/demo_Riemannian_quat_TPGMM01.cpp) | [] | TP-GMM on S3 (unit quaternion) with two frames |
-| [demo_Riemannian_sphere_GMM01.cpp](./src/demo_Riemannian_sphere_GMM01.cpp) | [[3]](#ref-3) | GMM for data on a sphere by relying on Riemannian manifold |
-| [demo_Riemannian_sphere_infHorLQR01.cpp](./src/demo_Riemannian_sphere_infHorLQR01.cpp) | [[3]](#ref-3) | Linear quadratic regulation on a sphere by relying on Riemannian manifold and infinite-horizon LQR |
-| [demo_Riemannian_sphere_product01.cpp](./src/demo_Riemannian_sphere_product01.cpp) | [] | Gaussian product on sphere |
-| [demo_Riemannian_sphere_TPGMM01.cpp](./src/demo_Riemannian_sphere_TPGMM01.cpp) | [[3]](#ref-3) | TP-GMM for data on a sphere by relying on Riemannian manifold (with single frame) |
-| [demo_TPbatchLQR01.cpp](./src/demo_TPbatchLQR01.cpp) | [[1]](#ref-1) | Linear quadratic control (unconstrained linear MPC) acting in multiple frames, which is equivalent to the fusion of Gaussian controllers |
-| [demo_TPGMMProduct01.cpp](./src/demo_TPGMMProduct01.cpp) | [] | Product of Gaussians for a Task-Parametrized GMM |
-| [demo_TPGMR01.cpp](./src/demo_TPGMR01.cpp) | [] | Task-Parametrized GMM with GMR (time as input), the model is able to adapt to continuously changing task parameters. |
diff --git a/src/demo_ergodicControl_2D01.cpp b/src/demo_ergodicControl_2D01.cpp
new file mode 100644
index 0000000..508c375
--- /dev/null
+++ b/src/demo_ergodicControl_2D01.cpp
@@ -0,0 +1,586 @@
+/*
+ * demo_ergodicControl_2D01.cpp
+ *
+ * 2D ergodic control with spectral multiscale coverage (SMC) algorithm, based on
+ * "Spectral Multiscale Coverage: A Uniform Coverage Algorithm for Mobile Sensor Networks"
+ * by George Mathew and Igor Mezic (http://www.geoggy.net/resources/SMC_CDC09.pdf)
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+#include <stdio.h>
+#include <armadillo>
+#include <mvn.h>
+#include <mpc_utils.h>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <GLFW/glfw3.h>
+
+using namespace arma;
+
+
+/***************************** ALGORITHM SECTION *****************************/
+
+typedef std::vector<vec> vector_list_t;
+typedef std::vector<mat> matrix_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the parameters used by the algorithm. Some of them are
+// modifiable through the UI, others are hard-coded.
+//-----------------------------------------------------------------------------
+struct parameters_t {
+ int nb_gaussians; // Number of gaussians that control the trajectory
+ int nb_data; // Number of datapoints
+ int nb_fct[2]; // Number of basis functions along x and y
+ int nb_res; // Resolution of discretization for the computation of
+ // Fourier coefficients of coverage distribution
+ float dt; // Time step
+ mat xlim; // Domain limits
+};
+
+
+//-----------------------------------------------------------------------------
+// Implementation of the algorithm
+//-----------------------------------------------------------------------------
+std::tuple<mat, mat> compute(const parameters_t& parameters, const mat& mu,
+ const cube& sigma) {
+
+ const int nb_var = 2; // Dimension of datapoint
+ const float sp = ((float) nb_var + 1.0f) / 2.0f; // Sobolev norm parameter
+
+ vec xsiz({ parameters.xlim(0, 1) - parameters.xlim(0, 0),
+ parameters.xlim(1, 1) - parameters.xlim(1, 0)
+ }); // Domain size
+
+ vec dx = xsiz / parameters.nb_res; // Spatial increments
+ vec x({0.1f, 0.3f}); // Initial position
+
+
+ // Basis functions (Fourier coefficients of coverage distribution)
+ //----------------------------------------------------------------
+ vector_list_t rg;
+ rg.push_back(linspace<vec>(0, parameters.nb_fct[0] - 1, parameters.nb_fct[0]));
+ rg.push_back(linspace<vec>(0, parameters.nb_fct[1] - 1, parameters.nb_fct[1]));
+
+ mat KX1 = repmat(rg[0], 1, parameters.nb_fct[1]);
+ mat KX2 = repmat(rg[1].t(), parameters.nb_fct[0], 1);
+
+ mat LK = pow(pow(KX1, 2) + pow(KX2, 2) + 1, -sp); // Weighting matrix
+
+ mat HK = join_vert(mat({1.0}), sqrt(0.5) * ones(parameters.nb_fct[0] - 1)) *
+ join_vert(mat({1.0}), sqrt(0.5) * ones(parameters.nb_fct[1] - 1)).t() *
+ sqrt(xsiz(0) * xsiz(1)); // Normalizing matrix
+
+ mat X = repmat(linspace<vec>(parameters.xlim(0, 0), parameters.xlim(0, 1) - dx(0), parameters.nb_res).t(),
+ parameters.nb_res, 1);
+
+ mat Y = repmat(linspace<vec>(parameters.xlim(1, 0), parameters.xlim(1, 1) - dx(1), parameters.nb_res),
+ 1, parameters.nb_res);
+
+ // Desired spatial distribution as mixture of Gaussians
+ cube G_(X.n_rows, X.n_cols, mu.n_cols);
+
+ mat XY(X.n_elem, 2);
+ XY(span(0, X.n_elem - 1), 0) = reshape(X, X.n_elem, 1);
+ XY(span(0, X.n_elem - 1), 1) = reshape(Y, Y.n_elem, 1);
+
+ for (int k = 0; k < mu.n_cols; ++k) {
+ G_.slice(k) = reshape(
+ mvn::getPDFValue(mu(span::all, k), sigma.slice(k), XY.t()),
+ X.n_rows, X.n_cols
+ ).t() / mu.n_cols;
+ }
+
+ mat G = sum(G_, 2);
+ G /= accu(G); // Spatial distribution
+
+ // Computation of phi_k by discretization
+ // mat phi = zeros(parameters.nb_fct[0], parameters.nb_fct[1]);
+ // for (int kx = 0; kx < parameters.nb_fct[0]; ++kx) {
+ // for (int ky = 0; ky < parameters.nb_fct[1]; ++ky) {
+ // phi(kx, ky) = accu(G % cos(X * kx * datum::pi / xsiz(0)) % cos(Y * ky * datum::pi / xsiz(1))) /
+ // HK(kx, ky); // Fourier coefficients of spatial distribution
+ // }
+ // }
+
+ // Explicit description of phi_k by exploiting the Fourier transform properties
+ // of Gaussians
+ mat w1 = KX1 * datum::pi / xsiz(0);
+ mat w2 = KX2 * datum::pi / xsiz(1);
+
+ mat w(2, w1.n_elem);
+ w(0, span::all) = reshape(w1, 1, w1.n_elem);
+ w(1, span::all) = reshape(w2, 1, w1.n_elem);
+
+ // Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1])
+ mat op({ {-1, 1, -1, 1}, {-1, -1, 1, 1} });
+
+ // Compute phi_k
+ mat phi = zeros(parameters.nb_fct[0], parameters.nb_fct[1]);
+ for (int k = 0; k < mu.n_cols; ++k) {
+ for (int n = 0; n < op.n_cols; ++n) {
+ mat MuTmp = diagmat(op(span::all, n)) * mu(span::all, k);
+ mat SigmaTmp = diagmat(op(span::all, n)) * sigma.slice(k) * diagmat(op(span::all, n)).t();
+ phi = phi + reshape(cos(w.t() * MuTmp) % diagvec(exp(-0.5 * w.t() * SigmaTmp * w)), size(HK));
+ }
+ }
+ phi = phi / HK / mu.n_cols / op.n_cols;
+
+
+ // Ergodic control with spectral multiscale coverage (SMC) algorithm
+ //------------------------------------------------------------------
+ mat Ck = zeros(parameters.nb_fct[0], parameters.nb_fct[1]);
+
+ mat result = zeros(nb_var, parameters.nb_data);
+
+ for (int t = 0; t < parameters.nb_data; ++t) {
+
+ // Log data
+ result(span::all, t) = x;
+
+ // Updating Fourier cosine coefficients of coverage distribution for each dimension
+ vector_list_t cx;
+ vector_list_t dcx;
+
+ for (int i = 0; i < nb_var; ++i) {
+ cx.push_back(cos(rg[i] * datum::pi * (x(i) - parameters.xlim(i, 0)) / xsiz(i)));
+ dcx.push_back(sin(rg[i] * datum::pi * (x(i) - parameters.xlim(i, 0)) / xsiz(i)) % rg[i] * datum::pi / xsiz(i));
+ }
+
+ // Fourier cosine coefficients along trajectory
+ Ck = Ck + (repmat(cx[0], 1, parameters.nb_fct[1]) % repmat(cx[1].t(), parameters.nb_fct[0], 1)) / HK * parameters.dt;
+
+ // SMC feedback control law
+ dx(0) = accu((LK / HK) % (Ck - phi * (t + 1) * parameters.dt) %
+ (repmat(dcx[0], 1, parameters.nb_fct[1]) % repmat(cx[1].t(), parameters.nb_fct[0], 1)));
+
+ dx(1) = accu((LK / HK) % (Ck - phi * (t + 1) * parameters.dt) %
+ (repmat(cx[0], 1, parameters.nb_fct[1]) % repmat(dcx[1].t(), parameters.nb_fct[0], 1)));
+
+ x = x + dx * parameters.dt;
+ }
+
+ return std::make_tuple(result, G);
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description){
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+//-----------------------------------------------
+
+std::tuple<vec, mat> trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
+ const gfx2::window_size_t& window_size,
+ int background_size) {
+
+ vec mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
+ window_size);
+
+ mu(0) *= (float) window_size.fb_width / background_size;
+ mu(1) *= (float) window_size.fb_height / background_size;
+
+ vec t_x({
+ gaussian_transforms.x.x * window_size.scale_x(),
+ gaussian_transforms.x.y * window_size.scale_y()
+ });
+
+ vec t_y({
+ gaussian_transforms.y.x * window_size.scale_x(),
+ gaussian_transforms.y.y * window_size.scale_y()
+ });
+
+ mat RG = {
+ { t_x(0), t_y(0) },
+ { -t_x(1), -t_y(1) }
+ };
+
+ mat sigma = RG * RG.t();
+
+ return std::make_tuple(mu, sigma);
+}
+
+//-----------------------------------------------
+
+void gauss_to_trans2d(const vec& mu, const mat& sigma,
+ const gfx2::window_size_t& window_size, ui::Trans2d &t2d) {
+
+ vec ui_mu = gfx2::fb2ui_centered(mu, window_size);
+
+ t2d.pos.x = ui_mu(0);
+ t2d.pos.y = ui_mu(1);
+
+ mat V;
+ vec d;
+ eig_sym(d, V, sigma);
+ mat VD = V * diagmat(sqrt(d));
+
+ t2d.x.x = VD.col(0)(0) / window_size.scale_x();
+ t2d.x.y = VD.col(1)(0) / window_size.scale_y();
+ t2d.y.x = VD.col(0)(1) / window_size.scale_x();
+ t2d.y.y = VD.col(1)(1) / window_size.scale_y();
+}
+
+//-----------------------------------------------
+
+std::tuple<mat, cube, std::vector<ui::Trans2d> > create_random_gaussians(
+ const parameters_t& parameters, const gfx2::window_size_t& window_size,
+ int background_size) {
+
+ mat Mu = randu(2, parameters.nb_gaussians);
+ Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+
+ cube Sigma;
+ randomCovariances(
+ &Sigma, Mu,
+ vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+ 100 * (float) window_size.fb_height / window_size.win_height
+ }),
+ true, 0.0, 0.2
+ );
+
+ std::vector<ui::Trans2d> gaussians(parameters.nb_gaussians, ui::Trans2d());
+
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, gaussians[i]);
+
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
+
+ fvec x = rotation * fvec({ gaussians[i].x.x, gaussians[i].x.y, 0.0f, 0.0f });
+ gaussians[i].x.x = x(0);
+ gaussians[i].x.y = x(1);
+
+ fvec y = rotation * fvec({ gaussians[i].y.x, gaussians[i].y.y, 0.0f, 0.0f });
+ gaussians[i].y.x = y(0);
+ gaussians[i].y.y = y(1);
+
+ vec mu_;
+ mat sigma_;
+
+ std::tie(mu_, sigma_) = trans2d_to_gauss(
+ gaussians[i], window_size, background_size
+ );
+
+ Sigma.slice(i) = sigma_;
+ }
+
+ return std::make_tuple(Mu, Sigma, gaussians);
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv){
+
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_gaussians = 2;
+ parameters.nb_data = 2000;
+ parameters.nb_fct[0] = 20;
+ parameters.nb_fct[1] = 20;
+ parameters.nb_res = 100;
+ parameters.dt = 0.01f;
+ parameters.xlim = mat({ { 0.0f, 1.0f }, { 0.0f, 1.0f } } );
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+
+
+ // Initialise GLFW
+ glfwSetErrorCallback(error_callback);
+
+ if (!glfwInit())
+ return -1;
+
+ glfwWindowHint(GLFW_SAMPLES, 4);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
+
+ // Open a window and create its OpenGL context
+ GLFWwindow* window = gfx2::create_window_at_optimal_size(
+ "Demo 2D ergodic control",
+ window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+
+ // Setup OpenGL
+ gfx2::init();
+ glEnable(GL_DEPTH_TEST);
+ glEnable(GL_CULL_FACE);
+ glEnable(GL_LINE_SMOOTH);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ // Setup ImGui
+ ImGui::CreateContext();
+ ImGui_ImplGlfwGL2_Init(window, true);
+
+
+ // Gaussian widgets
+ std::vector<ui::Trans2d> gaussians;
+
+ // Covariances
+ mat Mu;
+ cube Sigma;
+
+ // Main loop
+ mat result;
+ mat G;
+ const float speed = 1.0f / 20.0f;
+ float t = 0.0f;
+ bool must_recompute = false;
+ int background_size;
+
+ gfx2::texture_t background_texture = {0};
+
+ while (!glfwWindowShouldClose(window)) {
+ glfwPollEvents();
+
+ // Handling of the resizing of the window
+ gfx2::window_size_t previous_size;
+
+ gfx2::window_result_t window_result =
+ gfx2::handle_window_resizing(window, &window_size, &previous_size);
+
+ if (window_result == gfx2::INVALID_SIZE)
+ continue;
+
+ if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
+
+ background_size = std::max(window_size.fb_width, window_size.fb_height);
+
+ // Move and rescale the various objects so they stay in the window
+ if (window_result == gfx2::WINDOW_RESIZED) {
+ float scale = std::min((float) window_size.fb_width / previous_size.fb_width,
+ (float) window_size.fb_height / previous_size.fb_height);
+
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ arma::vec target = gfx2::fb2ui_centered(
+ gfx2::ui2fb_centered({gaussians[i].pos.x, gaussians[i].pos.y},
+ previous_size.win_width, previous_size.win_height,
+ previous_size.fb_width, previous_size.fb_height) * scale,
+ window_size.win_width, window_size.win_height,
+ window_size.fb_width, window_size.fb_height
+ );
+
+ gaussians[i].pos.x = target(0);
+ gaussians[i].pos.y = target(1);
+
+ gaussians[i].x.x *= scale;
+ gaussians[i].x.y *= scale;
+ gaussians[i].y.x *= scale;
+ gaussians[i].y.y *= scale;
+ }
+ }
+
+ // At the very first frame: random initialisation of the gaussians (taking 4K
+ // screens into account)
+ else if (window_result == gfx2::WINDOW_READY) {
+ std::tie(Mu, Sigma, gaussians) = create_random_gaussians(
+ parameters, window_size, background_size
+ );
+ }
+
+ must_recompute = true;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-window_size.fb_width / 2, window_size.fb_width / 2,
+ -window_size.fb_height / 2, window_size.fb_height / 2,
+ -1.0f, 1.0f
+ );
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ if (result.n_cols > 0) {
+
+ double max = G.max();
+ double min = G.min();
+
+ if (background_texture.width == 0) {
+ background_texture = gfx2::create_texture(
+ parameters.nb_res, parameters.nb_res, GL_RGB, GL_FLOAT
+ );
+
+ for (int j = 0; j < parameters.nb_res; ++j) {
+ for (int i = 0; i < parameters.nb_res; ++i) {
+ float color = 1.0f - (G(i, j) - min) / (max - min);
+
+ background_texture.pixels_f[(j * parameters.nb_res + i) * 3] = color;
+ background_texture.pixels_f[(j * parameters.nb_res + i) * 3 + 1] = 1.0f;
+ background_texture.pixels_f[(j * parameters.nb_res + i) * 3 + 2] = color;
+ }
+ }
+ }
+
+ gfx2::draw_rectangle(background_texture, background_size, background_size);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ mat result2(result.n_rows, result.n_cols);
+ result2(0, span::all) = (result(0, span::all) - 0.5) * background_size;
+ result2(1, span::all) = (result(1, span::all) - 0.5) * background_size;
+
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), result2);
+
+ int current_index = t * result2.n_cols;
+
+ if (current_index > 0) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ glLineWidth(4.0f);
+ gfx2::draw_line(fvec({ 1.0f, 0.0f, 0.0f }), result2(span::all, span(0, current_index)));
+ glLineWidth(1.0f);
+ }
+
+ fvec current_position = zeros<fvec>(3);
+ current_position(0) = result2(0, current_index);
+ current_position(1) = result2(1, current_index);
+
+ gfx2::draw_rectangle(fvec({ 1.0f, 0.0f, 0.0f }), 10.0f, 10.0f, current_position);
+ }
+
+
+ // Control panel GUI
+ ImGui::SetNextWindowSize(ImVec2(650, 130));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Control Panel", NULL,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
+
+ int previous_nb_gaussians = parameters.nb_gaussians;
+ int previous_nb_data = parameters.nb_data;
+ int previous_nb_fct_x = parameters.nb_fct[0];
+ int previous_nb_fct_y = parameters.nb_fct[1];
+ int previous_nb_res = parameters.nb_res;
+
+ ImGui::SliderInt("Nb gaussians", ¶meters.nb_gaussians, 1, 10);
+ ImGui::SliderInt("Nb data", ¶meters.nb_data, 500, 10000);
+ ImGui::SliderInt("Nb basis functions along X", ¶meters.nb_fct[0], 5, 100);
+ ImGui::SliderInt("Nb basis functions along Y", ¶meters.nb_fct[1], 5, 100);
+ ImGui::SliderInt("Resolution of discretization", ¶meters.nb_res, 20, 500);
+
+ if ((parameters.nb_gaussians != previous_nb_gaussians) ||
+ (parameters.nb_data != previous_nb_data) ||
+ (parameters.nb_fct[0] != previous_nb_fct_x) ||
+ (parameters.nb_fct[1] != previous_nb_fct_y) ||
+ (parameters.nb_res != previous_nb_res)) {
+
+ must_recompute = true;
+ }
+
+ ImGui::End();
+
+
+ // Gaussian widgets
+ if (parameters.nb_gaussians != gaussians.size()) {
+ std::tie(Mu, Sigma, gaussians) = create_random_gaussians(
+ parameters, window_size, background_size
+ );
+ }
+
+ ui::begin("Gaussian");
+
+ if (!gaussians.empty()) {
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ ui::Trans2d previous_gaussian = gaussians[i];
+
+ gaussians[i] = ui::affineSimple(i, gaussians[i]);
+
+ vec mu;
+ mat sigma;
+
+ std::tie(mu, sigma) = trans2d_to_gauss(
+ gaussians[i], window_size, background_size
+ );
+
+ Mu.col(i) = mu;
+ Sigma.slice(i) = sigma;
+
+ must_recompute = must_recompute ||
+ !gfx2::is_close(gaussians[i].pos.x, previous_gaussian.pos.x) ||
+ !gfx2::is_close(gaussians[i].pos.y, previous_gaussian.pos.y) ||
+ !gfx2::is_close(gaussians[i].x.x, previous_gaussian.x.x) ||
+ !gfx2::is_close(gaussians[i].x.y, previous_gaussian.x.y) ||
+ !gfx2::is_close(gaussians[i].y.x, previous_gaussian.y.x) ||
+ !gfx2::is_close(gaussians[i].y.y, previous_gaussian.y.y);
+ }
+ }
+
+ ui::end();
+
+
+ // Redo the computation when necessary
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ mat mu = Mu;
+ mu(0, span::all) = mu(0, span::all) / window_size.fb_width + 0.5;
+ mu(1, span::all) = mu(1, span::all) / window_size.fb_height + 0.5;
+
+ mat scaling({
+ { 1.0 / background_size, 0.0 },
+ { 0.0, 1.0 / background_size }
+ });
+
+ cube sigma(Sigma.n_rows, Sigma.n_cols, Sigma.n_slices);
+ for (int i = 0; i < Sigma.n_slices; ++i)
+ sigma.slice(i) = scaling * Sigma.slice(i) * scaling.t();
+
+ std::tie(result, G) = compute(parameters, mu, sigma);
+ t = 0.0f;
+
+ if (background_texture.width > 0)
+ gfx2::destroy(background_texture);
+
+ must_recompute = false;
+ }
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glPopMatrix();
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ t += speed * ImGui::GetIO().DeltaTime;
+ if (t >= 1.0f)
+ t = 0.0f;
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_ergodicControl_nD01.cpp b/src/demo_ergodicControl_nD01.cpp
new file mode 100644
index 0000000..3cde622
--- /dev/null
+++ b/src/demo_ergodicControl_nD01.cpp
@@ -0,0 +1,850 @@
+/*
+ * demo_ergodicControl_nD01.cpp
+ *
+ * nD ergodic control with spectral multiscale coverage (SMC) algorithm, based on
+ * "Spectral Multiscale Coverage: A Uniform Coverage Algorithm for Mobile Sensor Networks"
+ * by George Mathew and Igor Mezic (http://www.geoggy.net/resources/SMC_CDC09.pdf)
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+#include <stdio.h>
+#include <armadillo>
+#include <tensor.h>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <ImGuizmo.h>
+#include <GLFW/glfw3.h>
+
+using namespace arma;
+
+
+/***************************** ALGORITHM SECTION *****************************/
+
+typedef std::vector<vec> vector_list_t;
+typedef std::vector<mat> matrix_list_t;
+typedef std::vector<TensorUInt> tensor_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the parameters used by the algorithm. Some of them are
+// modifiable through the UI, others are hard-coded.
+//-----------------------------------------------------------------------------
+struct parameters_t {
+ int nb_gaussians; // Number of gaussians that control the trajectory
+ int nb_data; // Number of datapoints
+ int nb_fct; // Number of basis functions along each dimension
+ int nb_var; // Dimension of datapoint
+ float dt; // Time step
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a 3D gaussian
+//-----------------------------------------------------------------------------
+struct gaussian_t {
+ vec mu;
+ mat sigma;
+
+ fmat transforms; // Used to display the 3D widget
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of 3D gaussians
+//-----------------------------------------------------------------------------
+typedef std::vector<gaussian_t> gaussian_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Given a vector representing indices in a N-D tensor (with all dimensions
+// having the same number of elements, <size>), update it to contain the next
+// indices (assuming an iteration over all possible combinations). If
+// <fixed_dim> contains a valid dimension number, that dimension isn't
+// incremented.
+//
+// Returns false when the iteration is done.
+//-----------------------------------------------------------------------------
+bool increment_subscripts(ivec &subscripts, int size, int fixed_dim) {
+ int dim = 0;
+
+ while (dim < subscripts.n_elem) {
+ if (dim == fixed_dim) {
+ ++dim;
+ continue;
+ }
+
+ subscripts(dim) += 1;
+ if (subscripts(dim) >= size) {
+ subscripts(dim) = 0;
+ ++dim;
+ continue;
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Returns a list of <nb_var> <nb_var>-D tensors, each dimension having a size
+// of <lst.n_elem>.
+//
+// The values of <lst> are used to fill the tensors, like in the following
+// example.
+//
+// With nb_var = 3 and lst = [0..2]:
+//
+// result[0]:
+// slice 0 = [ slice 1 = [ slice 2 = [
+// 0 0 0 0 0 0 0 0 0
+// 1 1 1 1 1 1 1 1 1
+// 2 2 2 2 2 2 2 2 2
+// ] ] ]
+//
+// result[1]:
+// slice 0 = [ slice 1 = [ slice 2 = [
+// 0 1 2 0 1 2 0 1 2
+// 0 1 2 0 1 2 0 1 2
+// 0 1 2 0 1 2 0 1 2
+// ] ] ]
+//
+// result[2]:
+// slice 0 = [ slice 1 = [ slice 2 = [
+// 0 0 0 1 1 1 2 2 2
+// 0 0 0 1 1 1 2 2 2
+// 0 0 0 1 1 1 2 2 2
+// ] ] ]
+//
+//-----------------------------------------------------------------------------
+tensor_list_t ndarr(const uvec& lst, int nb_var) {
+ tensor_list_t result;
+
+ ivec dims = ones<ivec>(nb_var) * lst.n_elem;
+
+ for (int n = 0; n < nb_var; ++n) {
+ TensorUInt tensor(dims);
+
+ for (int i = 0; i < lst.n_elem; ++i) {
+
+ ivec subscripts = zeros<ivec>(nb_var);
+ subscripts(n) = i;
+
+ do {
+ tensor.set(tensor.indice(subscripts), lst(i));
+ } while (increment_subscripts(subscripts, lst.n_elem, n));
+ }
+
+ result.push_back(tensor);
+ }
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Returns an N-by-N Hadamard matrix (where N is the form 2^k)
+//-----------------------------------------------------------------------------
+mat hadamard(int N) {
+ const mat H2({
+ { 1, 1 },
+ { 1, -1 }
+ });
+
+ if (N == 2)
+ return H2;
+
+ return kron(H2, hadamard(N / 2));
+}
+
+
+//-----------------------------------------------------------------------------
+// Implementation of the algorithm
+//-----------------------------------------------------------------------------
+mat compute(const parameters_t& parameters, const mat& mu, const cube& sigma) {
+
+ const float sp = ((float) parameters.nb_var + 1.0f) / 2.0f; // Sobolev norm parameter
+
+ const float xsize = 1.0; // Domain size
+
+ // Initial position
+ vec x = zeros(parameters.nb_var);
+ x(span(0, 2)) = vec({ 0.7f, 0.1f, 0.5f });
+
+
+ // Basis functions (Fourier coefficients of coverage distribution)
+ //----------------------------------------------------------------
+ tensor_list_t Karr = ndarr(
+ linspace<uvec>(0, parameters.nb_fct - 1, parameters.nb_fct),
+ parameters.nb_var
+ );
+
+ mat rg = repmat(
+ linspace<rowvec>(0, parameters.nb_fct - 1, parameters.nb_fct),
+ parameters.nb_var, 1
+ );
+
+ TensorDouble stmp(Karr[0].dims);
+ for (int n = 0; n < parameters.nb_var; ++n)
+ stmp.data += pow(conv_to<vec>::from(Karr[n].data), 2);
+
+ // Weighting matrix
+ TensorDouble LK(stmp.dims);
+ LK.data = pow(stmp.data + 1, -sp);
+
+ vec hk = join_vert(vec({1.0}), sqrt(0.5) * ones(parameters.nb_fct - 1));
+
+ TensorDouble HK(Karr[0].dims);
+ HK.data = ones(HK.size);
+ for (int n = 0; n < parameters.nb_var; ++n)
+ HK.data = HK.data % hk.elem(Karr[n].data);
+
+
+ // Desired spatial distribution as mixture of Gaussians
+ mat w(parameters.nb_var, Karr[0].size);
+ for (int n = 0; n < parameters.nb_var; ++n)
+ w(n, span::all) = conv_to<vec>::from(Karr[n].data).t() * datum::pi / xsize;
+
+
+ // Enumerate symmetry operations for <nb_var>-D signal
+ mat op = hadamard(pow(2, parameters.nb_var - 1));
+ op = op(span(0, parameters.nb_var - 1), span::all);
+
+
+ // Compute phi_k
+ TensorDouble phi(HK.dims);
+ phi.data = zeros(phi.size);
+ for (int k = 0; k < mu.n_cols; ++k) {
+ for (int n = 0; n < op.n_cols; ++n) {
+ mat MuTmp = diagmat(op(span::all, n)) * mu(span::all, k);
+ mat SigmaTmp = diagmat(op(span::all, n)) * sigma.slice(k) * diagmat(op(span::all, n)).t();
+ phi.data = phi.data + cos(w.t() * MuTmp) % diagvec(exp(-0.5 * w.t() * SigmaTmp * w));
+ }
+ }
+ phi.data = phi.data / HK.data / mu.n_cols / op.n_cols;
+
+
+ // Ergodic control with spectral multiscale coverage (SMC) algorithm
+ //------------------------------------------------------------------
+ TensorDouble Ck(HK.dims);
+ Ck.data = zeros(Ck.size);
+
+ mat result = zeros(parameters.nb_var, parameters.nb_data);
+
+ for (int t = 0; t < parameters.nb_data; ++t) {
+
+ // Log data
+ result(span::all, t) = x;
+
+ // Updating Fourier coefficients of coverage distribution
+ mat cx = cos(rg * datum::pi / xsize % repmat(x, 1, parameters.nb_fct));
+ mat dcx = sin(rg * datum::pi / xsize % repmat(x, 1, parameters.nb_fct)) % rg * datum::pi / xsize;
+
+ vec Mtmp = ones(pow(parameters.nb_fct, parameters.nb_var));
+ for (int n = 0; n < parameters.nb_var; ++n)
+ Mtmp = Mtmp % rowvec(cx.row(n)).elem(Karr[n].data);
+
+ // Fourier coefficients along trajectory
+ Ck.data = Ck.data + Mtmp / HK.data * parameters.dt;
+
+ // SMC feedback control law
+ vec dx(parameters.nb_var);
+
+ for (int n = 0; n < parameters.nb_var; ++n) {
+ Mtmp = ones(pow(parameters.nb_fct, parameters.nb_var));
+ for (int m = 0; m < parameters.nb_var; ++m) {
+ if (m == n)
+ Mtmp = Mtmp % rowvec(dcx.row(m)).elem(Karr[m].data);
+ else
+ Mtmp = Mtmp % rowvec(cx.row(m)).elem(Karr[m].data);
+ }
+
+ Mtmp = LK.data / HK.data % (Ck.data - phi.data * t * parameters.dt) % Mtmp;
+ dx(n) = sum(Mtmp);
+ }
+
+ x = x + dx * parameters.dt;
+ }
+
+ return result;
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description){
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+
+//-----------------------------------------------------------------------------
+// Colors of the displayed gaussians
+//-----------------------------------------------------------------------------
+const fmat COLORS({
+ { 0.0f, 0.0f, 1.0f },
+ { 0.0f, 0.5f, 0.0f },
+ { 1.0f, 0.0f, 0.0f },
+ { 0.0f, 0.75f, 0.75f },
+ { 0.75f, 0.0f, 0.75f },
+ { 0.75f, 0.75f, 0.0f },
+});
+
+
+//-----------------------------------------------
+
+
+gaussian_list_t create_random_gaussians(
+ const parameters_t& parameters, const gfx2::window_size_t& window_size) {
+
+ gaussian_list_t gaussians;
+
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ gaussian_t gaussian;
+ gaussian.mu = randu(3);
+
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi) *
+ gfx2::rotate(fvec({ 1.0f, 0.0f, 0.0f }), randu() * 2 * datum::pi);
+
+ mat RG = zeros(3, 3);
+ RG(span::all, 0) = vec(rotation * vec({ randu() * 0.2 + 0.05, 0.0, 0.0, 0.0 })).rows(0, 2);
+ RG(span::all, 1) = vec(rotation * vec({ 0.0, randu() * 0.2 + 0.05, 0.0, 0.0 })).rows(0, 2);
+ RG(span::all, 2) = vec(rotation * vec({ 0.0, 0.0, randu() * 0.2 + 0.05, 0.0 })).rows(0, 2);
+
+ gaussian.sigma = RG * RG.t();
+
+ gaussian.transforms = eye<fmat>(4, 4);
+ gaussian.transforms(span(0, 2), span(0, 2)) = conv_to<fmat>::from(RG);
+ gaussian.transforms(span(0, 2), 3) = conv_to<fvec>::from(gaussian.mu);
+
+ gaussians.push_back(gaussian);
+ }
+
+ return gaussians;
+}
+
+
+//-----------------------------------------------------------------------------
+// Render the 3D scene
+//-----------------------------------------------------------------------------
+bool draw_scene(const gfx2::window_size_t& window_size, const fmat& projection,
+ const gfx2::camera_t& camera, const mat& result, float t,
+ gaussian_list_t &gaussians, int ¤t_gaussian,
+ ImGuizmo::OPERATION transforms_operation) {
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMultMatrixf(projection.memptr());
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ fmat view = gfx2::view_matrix(camera);
+ glMultMatrixf(view.memptr());
+
+
+ // Draw the axes
+ gfx2::draw_line(
+ fvec({ 1.0f, 0.0f, 0.0f }),
+ mat({
+ { 0.0, 1.0 },
+ { 0.0, 0.0 },
+ { 0.0, 0.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.0f, 1.0f, 0.0f }),
+ mat({
+ { 0.0, 0.0 },
+ { 0.0, 1.0 },
+ { 0.0, 0.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.0f, 0.0f, 1.0f }),
+ mat({
+ { 0.0, 0.0 },
+ { 0.0, 0.0 },
+ { 0.0, 1.0 },
+ })
+ );
+
+
+ // Draw the boundaries
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 0.0, 1.0 },
+ { 0.0, 0.0 },
+ { 1.0, 1.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 0.0, 1.0 },
+ { 1.0, 1.0 },
+ { 0.0, 0.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 0.0, 1.0 },
+ { 1.0, 1.0 },
+ { 1.0, 1.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 1.0, 1.0 },
+ { 0.0, 1.0 },
+ { 0.0, 0.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 1.0, 1.0 },
+ { 0.0, 1.0 },
+ { 1.0, 1.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 0.0, 0.0 },
+ { 0.0, 1.0 },
+ { 1.0, 1.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 1.0, 1.0 },
+ { 0.0, 0.0 },
+ { 0.0, 1.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 0.0, 0.0 },
+ { 1.0, 1.0 },
+ { 0.0, 1.0 },
+ })
+ );
+
+ gfx2::draw_line(
+ fvec({ 0.8f, 0.8f, 0.8f }),
+ mat({
+ { 1.0, 1.0 },
+ { 1.0, 1.0 },
+ { 0.0, 1.0 },
+ })
+ );
+
+
+ // Draw the gaussians
+ if (!gaussians.empty()) {
+ for (int i = 0; i < gaussians.size(); ++i) {
+ gfx2::draw_gaussian_3D(COLORS.row(i % COLORS.n_rows).t(), gaussians[i].mu,
+ gaussians[i].sigma);
+ }
+ }
+
+
+ // Draw the results
+ if (result.n_cols > 0) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), result(span(0, 2), span::all));
+
+ int current_index = t * result.n_cols;
+
+ if (current_index > 0) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ glLineWidth(4.0f);
+ gfx2::draw_line(fvec({ 1.0f, 0.0f, 0.0f }), result(span(0, 2), span(0, current_index)));
+ glLineWidth(1.0f);
+ }
+ }
+
+
+ // Draw the gizmo allowing to manipulate the gaussians
+ if (!gaussians.empty()) {
+
+ // Draw a selection circle at the center of each gaussian
+ ImGui::PushID("gaussians");
+ ImGui::SetNextWindowPos(ImVec2(0,0), ImGuiCond_Always);
+ ImGui::SetNextWindowSize(ImGui::GetIO().DisplaySize, ImGuiCond_Always);
+ ImGui::SetNextWindowBgAlpha(0.0);
+
+ ImGui::Begin("gaussians", 0, ImGuiWindowFlags_NoTitleBar |
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove |
+ ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoBringToFrontOnFocus);
+
+ ImGui::SetNextWindowBgAlpha(1.0);
+
+ for (int i = 0; i < gaussians.size(); ++i) {
+ if (i == current_gaussian)
+ continue;
+
+ fvec coords_world(4);
+ coords_world(span(0, 2)) = conv_to<fvec>::from(gaussians[i].mu);
+ coords_world(3) = 1.0f;
+
+ fvec coords_camera = view * coords_world;
+
+ fvec coords_screen = projection * coords_camera;
+ coords_screen /= coords_screen(3);
+
+ ivec coords_ui = conv_to<ivec>::from(
+ (coords_screen(span(0, 1)) + 1.0f) / 2.0f %
+ fvec({ (float) window_size.win_width, (float) window_size.win_height })
+ );
+
+ coords_ui(1) = window_size.win_height - coords_ui(1);
+
+ ImDrawList* draw_list = ImGui::GetWindowDrawList();
+
+ const int radius = 6;
+
+ bool mouse_is_over = ImGui::IsMouseHoveringRect(
+ ImVec2(coords_ui(0) - radius, coords_ui(1) - radius),
+ ImVec2(coords_ui(0) + radius, coords_ui(1) + radius)
+ );
+
+ draw_list->AddCircleFilled(ImVec2(coords_ui(0), coords_ui(1)), radius,
+ (mouse_is_over ? 0xFF00A5FF : 0xFFFFFFFF));
+ draw_list->AddCircle(ImVec2(coords_ui(0), coords_ui(1)), radius, 0xFF000000);
+
+ if (mouse_is_over && ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
+ current_gaussian = i;
+ ImGui::End();
+ ImGui::PopID();
+ return false;
+ }
+ }
+
+ ImGui::End();
+ ImGui::PopID();
+
+
+ // Draw the 3D manipulator
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ ImGuizmo::Enable(true);
+ ImGuizmo::SetRect(0, 0, window_size.win_width, window_size.win_height);
+
+ fmat diff(4, 4);
+ ImGuizmo::Manipulate(
+ view.memptr(), projection.memptr(), transforms_operation,
+ ImGuizmo::LOCAL, gaussians[current_gaussian].transforms.memptr(), diff.memptr()
+ );
+
+
+ // Convert the transforms to mu and sigma
+ if (any(any(diff - eye(4, 4)))) {
+ gaussians[current_gaussian].mu = conv_to<vec>::from(
+ gaussians[current_gaussian].transforms(span(0, 2), 3)
+ );
+ }
+
+ //-- Ensures mu stays in the boundaries
+ if (ImGui::IsMouseReleased(GLFW_MOUSE_BUTTON_1)) {
+ if (gaussians[current_gaussian].mu(0) > 1.0f)
+ gaussians[current_gaussian].mu(0) = 1.0f;
+ else if (gaussians[current_gaussian].mu(0) < 0.0f)
+ gaussians[current_gaussian].mu(0) = 0.0f;
+
+ if (gaussians[current_gaussian].mu(1) > 1.0f)
+ gaussians[current_gaussian].mu(1) = 1.0f;
+ else if (gaussians[current_gaussian].mu(1) < 0.0f)
+ gaussians[current_gaussian].mu(1) = 0.0f;
+
+ if (gaussians[current_gaussian].mu(2) > 1.0f)
+ gaussians[current_gaussian].mu(2) = 1.0f;
+ else if (gaussians[current_gaussian].mu(2) < 0.0f)
+ gaussians[current_gaussian].mu(2) = 0.0f;
+
+ gaussians[current_gaussian].transforms(span(0, 2), 3) =
+ conv_to<fvec>::from(gaussians[current_gaussian].mu);
+ }
+
+ if (any(any(diff - eye(4, 4)))) {
+ mat rot_scale = conv_to<mat>::from(gaussians[current_gaussian].transforms(span(0, 2), span(0, 2)));
+
+ mat RG = zeros(3, 3);
+ RG(span::all, 0) = rot_scale * vec({ 1.0, 0.0, 0.0 });
+ RG(span::all, 1) = rot_scale * vec({ 0.0, 1.0, 0.0 });
+ RG(span::all, 2) = rot_scale * vec({ 0.0, 0.0, 1.0 });
+
+ gaussians[current_gaussian].sigma = RG * RG.t();
+
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv){
+
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_gaussians = 2;
+ parameters.nb_data = 2000;
+ parameters.nb_fct = 10;
+ parameters.nb_var = 3;
+ parameters.dt = 0.01f;
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+ int viewport_width = 0;
+ int viewport_height = 0;
+
+
+ // Initialise GLFW
+ glfwSetErrorCallback(error_callback);
+
+ if (!glfwInit())
+ return -1;
+
+ glfwWindowHint(GLFW_SAMPLES, 4);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
+
+ // Open a window and create its OpenGL context
+ GLFWwindow* window = gfx2::create_window_at_optimal_size(
+ "Demo nD ergodic control",
+ window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+
+ // Setup OpenGL
+ gfx2::init();
+ glEnable(GL_DEPTH_TEST);
+ glEnable(GL_CULL_FACE);
+ glEnable(GL_LINE_SMOOTH);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ // Setup ImGui
+ ImGui::CreateContext();
+ ImGui_ImplGlfwGL2_Init(window, true);
+
+
+ // Projection matrix
+ fmat projection;
+
+
+ // Camera
+ gfx2::camera_t camera(fvec({0.5f, 0.5f, 0.5f}), 2.0f);
+ gfx2::yaw(camera, gfx2::deg2rad(30.0f));
+ gfx2::pitch(camera, gfx2::deg2rad(-20.0f));
+
+
+ // Mouse control
+ double mouse_x, mouse_y, previous_mouse_x, previous_mouse_y;
+ bool rotating = false;
+ GLFWcursor* crosshair_cursor = glfwCreateStandardCursor(GLFW_HAND_CURSOR);
+ GLFWcursor* arrow_cursor = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
+
+ glfwGetCursorPos(window, &previous_mouse_x, &previous_mouse_y);
+
+
+ // 3D gaussians
+ gaussian_list_t gaussians;
+
+ // Main loop
+ mat result;
+ const float speed = 1.0f / 20.0f;
+ float t = 0.0f;
+ bool must_recompute = false;
+ ImGuizmo::OPERATION transforms_operation = ImGuizmo::TRANSLATE;
+ int current_gaussian = 0;
+
+ while (!glfwWindowShouldClose(window)) {
+ glfwPollEvents();
+
+ // Handling of the resizing of the window
+ gfx2::window_result_t window_result =
+ gfx2::handle_window_resizing(window, &window_size);
+
+ if (window_result == gfx2::INVALID_SIZE)
+ continue;
+
+ if ((window_result == gfx2::WINDOW_READY) || (window_result == gfx2::WINDOW_RESIZED)) {
+
+ viewport_width = window_size.fb_width / 3 - 1;
+ viewport_height = window_size.fb_height / 2 - 1;
+
+ // Update the projection matrix
+ projection = gfx2::perspective(
+ gfx2::deg2rad(60.0f),
+ (float) window_size.fb_width / (float) window_size.fb_height,
+ 0.1f, 10.0f
+ );
+
+ // At the very first frame: random initialisation of the gaussians (taking 4K
+ // screens into account)
+ if (window_result == gfx2::WINDOW_READY)
+ gaussians = create_random_gaussians(parameters, window_size);
+
+ must_recompute = true;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+ ImGuizmo::BeginFrame();
+
+ must_recompute = draw_scene(window_size, projection, camera, result, t,
+ gaussians, current_gaussian, transforms_operation
+ ) || must_recompute;
+
+
+ // Control panel GUI
+ ImGui::SetNextWindowSize(ImVec2(500, 106));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Control Panel", NULL,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
+
+ int previous_nb_gaussians = parameters.nb_gaussians;
+ int previous_nb_data = parameters.nb_data;
+ int previous_nb_fct = parameters.nb_fct;
+
+ ImGui::SliderInt("Nb gaussians", ¶meters.nb_gaussians, 1, 10);
+ ImGui::SliderInt("Nb data", ¶meters.nb_data, 500, 10000);
+ ImGui::SliderInt("Nb basis functions", ¶meters.nb_fct, 5, 20);
+
+ ImGui::Text("Current mode: ");
+ ImGui::SameLine();
+ ImGui::RadioButton("(T)ranslate", (int*) &transforms_operation, (int) ImGuizmo::TRANSLATE);
+ ImGui::SameLine();
+ ImGui::RadioButton("(R)otate", (int*) &transforms_operation, (int) ImGuizmo::ROTATE);
+ ImGui::SameLine();
+ ImGui::RadioButton("(S)cale", (int*) &transforms_operation, (int) ImGuizmo::SCALE);
+
+ if ((parameters.nb_gaussians != previous_nb_gaussians) ||
+ (parameters.nb_data != previous_nb_data) ||
+ (parameters.nb_fct != previous_nb_fct)) {
+
+ must_recompute = true;
+ }
+
+ ImGui::End();
+
+
+ // Gaussian widgets
+ if (parameters.nb_gaussians != gaussians.size()) {
+ gaussians = create_random_gaussians(parameters, window_size);
+ must_recompute = true;
+ }
+
+
+ // Redo the computation when necessary
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ mat mu = zeros(parameters.nb_var, parameters.nb_gaussians);
+ cube sigma = zeros(parameters.nb_var, parameters.nb_var, parameters.nb_gaussians);
+
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ mu(span(0, 2), i) = gaussians[i].mu;
+ sigma(span(0, 2), span(0, 2), span(i)) = gaussians[i].sigma;
+ }
+
+ result = compute(parameters, mu, sigma);
+ t = 0.0f;
+
+ must_recompute = false;
+ }
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glPopMatrix();
+ glfwSwapBuffers(window);
+
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+ if (ImGui::IsKeyPressed(GLFW_KEY_T))
+ transforms_operation = ImGuizmo::TRANSLATE;
+
+ else if (ImGui::IsKeyPressed(GLFW_KEY_R))
+ transforms_operation = ImGuizmo::ROTATE;
+
+ else if (ImGui::IsKeyPressed(GLFW_KEY_S))
+ transforms_operation = ImGuizmo::SCALE;
+
+
+ // Mouse input
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ //-- Right mouse button: rotation of the meshes while held down
+ if (ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_2)) {
+ if (!rotating) {
+ rotating = true;
+ glfwSetCursor(window, crosshair_cursor);
+ }
+
+ gfx2::yaw(camera, -0.2f * gfx2::deg2rad(mouse_x - previous_mouse_x));
+ gfx2::pitch(camera, -0.2f * gfx2::deg2rad(mouse_y - previous_mouse_y));
+
+ } else if (rotating) {
+ rotating = false;
+ glfwSetCursor(window, arrow_cursor);
+ }
+
+ previous_mouse_x = mouse_x;
+ previous_mouse_y = mouse_y;
+
+
+ t += speed * ImGui::GetIO().DeltaTime;
+ if (t >= 1.0f)
+ t = 0.0f;
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
--
GitLab