From 65b56cf3b0e618f1ceffaa89e160d0c790db256d Mon Sep 17 00:00:00 2001 From: scalinon <sylvain.calinon@idiap.ch> Date: Wed, 11 Mar 2020 09:36:40 +0100 Subject: [PATCH] Ref updated --- README.md | 6 +- demos/demo_Riemannian_Gdp_vecTransp01.m | 2 +- demos/demo_Riemannian_Hd_GMM01.m | 2 +- demos/demo_Riemannian_Hd_interp01.m | 2 +- demos/demo_Riemannian_S1_interp01.m | 2 +- demos/demo_Riemannian_S1_interp02.m | 2 +- demos/demo_Riemannian_S2_GMM01.m | 2 +- demos/demo_Riemannian_S2_GMR01.m | 2 +- demos/demo_Riemannian_S2_GMR02.m | 2 +- demos/demo_Riemannian_S2_GMR03.m | 2 +- demos/demo_Riemannian_S2_GMR04.m | 2 +- demos/demo_Riemannian_S2_GaussProd01.m | 2 +- demos/demo_Riemannian_S2_TPGMM01.m | 2 +- demos/demo_Riemannian_S2_TPGMM02.m | 2 +- demos/demo_Riemannian_S2_batchLQR01.m | 2 +- demos/demo_Riemannian_S2_batchLQR02.m | 2 +- demos/demo_Riemannian_S2_batchLQR03.m | 2 +- demos/demo_Riemannian_S2_batchLQR_Bezier01.m | 2 +- demos/demo_Riemannian_S2_infHorLQR01.m | 2 +- demos/demo_Riemannian_S2_vecTransp01.m | 2 +- demos/demo_Riemannian_S3_GMM01.m | 2 +- demos/demo_Riemannian_S3_GMR01.m | 2 +- demos/demo_Riemannian_S3_GMR02.m | 2 +- demos/demo_Riemannian_S3_infHorLQR01.m | 2 +- demos/demo_Riemannian_S3_interp01.m | 2 +- demos/demo_Riemannian_S3_vecTransp01.m | 2 +- demos/demo_Riemannian_SE2_GMM01.m | 2 +- demos/demo_Riemannian_SE2_interp01.m | 2 +- demos/demo_Riemannian_SOd_interp01.m | 2 +- demos/demo_Riemannian_Sd_GMM01.m | 2 +- demos/demo_Riemannian_Sd_GMM02.m | 2 +- demos/demo_Riemannian_Sd_GMR01.m | 2 +- demos/demo_Riemannian_Sd_GMR02.m | 2 +- demos/demo_Riemannian_Sd_GaussProd01.m | 2 +- demos/demo_Riemannian_Sd_MPC01.m | 2 +- demos/demo_Riemannian_Sd_MPC_infHor01.m | 2 +- demos/demo_Riemannian_Sd_interp01.m | 2 +- demos/demo_Riemannian_Sd_interp02.m | 2 +- demos/demo_Riemannian_Sd_vecTransp01.m | 2 +- demos/demo_Riemannian_Sd_vecTransp02.m | 2 +- demos/demo_Riemannian_pose_GMM01.m | 2 +- demos/demo_ergodicControl_1D01.m | 161 +++++++++++++++---- demos/demo_ergodicControl_2D01.m | 120 ++++++++++---- demos/demo_ergodicControl_3D01.m | 72 +++++++-- demos/demo_ergodicControl_nD01.m | 24 ++- 45 files changed, 329 insertions(+), 134 deletions(-) diff --git a/README.md b/README.md index cd0c8fa..5641eef 100644 --- a/README.md +++ b/README.md @@ -277,10 +277,10 @@ If you find PbDlib useful for your research, please cite the related publication </p> <p><a name="ref-3"> -[3] Calinon, S. and Jaquier, N. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM). +[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM). </a><br> -[[pdf]](http://calinon.ch/papers/Calinon-arXiv2019.pdf) -[[bib]](http://calinon.ch/papers/Calinon-arXiv2019.bib) +[[pdf]](http://calinon.ch/papers/Calinon-RAM2020.pdf) +[[bib]](http://calinon.ch/papers/Calinon-RAM2020.bib) <br><strong>(Ref. for Riemannian manifolds)</strong> </p> diff --git a/demos/demo_Riemannian_Gdp_vecTransp01.m b/demos/demo_Riemannian_Gdp_vecTransp01.m index 6120e82..f0d8029 100644 --- a/demos/demo_Riemannian_Gdp_vecTransp01.m +++ b/demos/demo_Riemannian_Gdp_vecTransp01.m @@ -3,7 +3,7 @@ function demo_Riemannian_Gdp_vecTransp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Hd_GMM01.m b/demos/demo_Riemannian_Hd_GMM01.m index 9775aff..1c787d9 100644 --- a/demos/demo_Riemannian_Hd_GMM01.m +++ b/demos/demo_Riemannian_Hd_GMM01.m @@ -3,7 +3,7 @@ function demo_Riemannian_Hd_GMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Hd_interp01.m b/demos/demo_Riemannian_Hd_interp01.m index c42a16d..3f72c2d 100644 --- a/demos/demo_Riemannian_Hd_interp01.m +++ b/demos/demo_Riemannian_Hd_interp01.m @@ -3,7 +3,7 @@ function demo_Riemannian_Hd_interp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S1_interp01.m b/demos/demo_Riemannian_S1_interp01.m index fe8d1cb..fbd0cb8 100644 --- a/demos/demo_Riemannian_S1_interp01.m +++ b/demos/demo_Riemannian_S1_interp01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S1_interp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S1_interp02.m b/demos/demo_Riemannian_S1_interp02.m index 81cdbfc..a3e5119 100644 --- a/demos/demo_Riemannian_S1_interp02.m +++ b/demos/demo_Riemannian_S1_interp02.m @@ -3,7 +3,7 @@ function demo_Riemannian_S1_interp02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_GMM01.m b/demos/demo_Riemannian_S2_GMM01.m index 2943cd2..f03349e 100644 --- a/demos/demo_Riemannian_S2_GMM01.m +++ b/demos/demo_Riemannian_S2_GMM01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_GMR01.m b/demos/demo_Riemannian_S2_GMR01.m index a9e0aba..4d50732 100644 --- a/demos/demo_Riemannian_S2_GMR01.m +++ b/demos/demo_Riemannian_S2_GMR01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_GMR02.m b/demos/demo_Riemannian_S2_GMR02.m index 44d70eb..e6ff655 100644 --- a/demos/demo_Riemannian_S2_GMR02.m +++ b/demos/demo_Riemannian_S2_GMR02.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_GMR03.m b/demos/demo_Riemannian_S2_GMR03.m index f676538..f75e191 100644 --- a/demos/demo_Riemannian_S2_GMR03.m +++ b/demos/demo_Riemannian_S2_GMR03.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR03 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_GMR04.m b/demos/demo_Riemannian_S2_GMR04.m index b40e14f..304b07d 100644 --- a/demos/demo_Riemannian_S2_GMR04.m +++ b/demos/demo_Riemannian_S2_GMR04.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR04 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_GaussProd01.m b/demos/demo_Riemannian_S2_GaussProd01.m index 33bfa74..7ac4ee6 100644 --- a/demos/demo_Riemannian_S2_GaussProd01.m +++ b/demos/demo_Riemannian_S2_GaussProd01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_GaussProd01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_TPGMM01.m b/demos/demo_Riemannian_S2_TPGMM01.m index 98d5057..0a50824 100644 --- a/demos/demo_Riemannian_S2_TPGMM01.m +++ b/demos/demo_Riemannian_S2_TPGMM01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_TPGMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_TPGMM02.m b/demos/demo_Riemannian_S2_TPGMM02.m index 58e707e..ef41fa2 100644 --- a/demos/demo_Riemannian_S2_TPGMM02.m +++ b/demos/demo_Riemannian_S2_TPGMM02.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_TPGMM02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_batchLQR01.m b/demos/demo_Riemannian_S2_batchLQR01.m index 1fc96ec..9abce44 100644 --- a/demos/demo_Riemannian_S2_batchLQR01.m +++ b/demos/demo_Riemannian_S2_batchLQR01.m @@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_batchLQR02.m b/demos/demo_Riemannian_S2_batchLQR02.m index 5601c20..380c787 100644 --- a/demos/demo_Riemannian_S2_batchLQR02.m +++ b/demos/demo_Riemannian_S2_batchLQR02.m @@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_batchLQR03.m b/demos/demo_Riemannian_S2_batchLQR03.m index cd1ec90..db0578c 100644 --- a/demos/demo_Riemannian_S2_batchLQR03.m +++ b/demos/demo_Riemannian_S2_batchLQR03.m @@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR03 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m index bfe5cb1..ef82dd7 100644 --- a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m +++ b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_batchLQR_Bezier01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_infHorLQR01.m b/demos/demo_Riemannian_S2_infHorLQR01.m index 0e3b1f8..361f79c 100644 --- a/demos/demo_Riemannian_S2_infHorLQR01.m +++ b/demos/demo_Riemannian_S2_infHorLQR01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_infHorLQR01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S2_vecTransp01.m b/demos/demo_Riemannian_S2_vecTransp01.m index 8fbc5b1..54dc29e 100644 --- a/demos/demo_Riemannian_S2_vecTransp01.m +++ b/demos/demo_Riemannian_S2_vecTransp01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S2_vecTransp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S3_GMM01.m b/demos/demo_Riemannian_S3_GMM01.m index 3f4f063..ecf960c 100644 --- a/demos/demo_Riemannian_S3_GMM01.m +++ b/demos/demo_Riemannian_S3_GMM01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S3_GMR01.m b/demos/demo_Riemannian_S3_GMR01.m index e6bebee..c0662cf 100644 --- a/demos/demo_Riemannian_S3_GMR01.m +++ b/demos/demo_Riemannian_S3_GMR01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMR01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S3_GMR02.m b/demos/demo_Riemannian_S3_GMR02.m index 6f9943e..6566f25 100644 --- a/demos/demo_Riemannian_S3_GMR02.m +++ b/demos/demo_Riemannian_S3_GMR02.m @@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMR02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S3_infHorLQR01.m b/demos/demo_Riemannian_S3_infHorLQR01.m index af84917..66e1278 100644 --- a/demos/demo_Riemannian_S3_infHorLQR01.m +++ b/demos/demo_Riemannian_S3_infHorLQR01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S3_infHorLQR01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S3_interp01.m b/demos/demo_Riemannian_S3_interp01.m index 5a8117c..979fc7e 100644 --- a/demos/demo_Riemannian_S3_interp01.m +++ b/demos/demo_Riemannian_S3_interp01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S3_interp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_S3_vecTransp01.m b/demos/demo_Riemannian_S3_vecTransp01.m index 68a1e65..84e9e97 100644 --- a/demos/demo_Riemannian_S3_vecTransp01.m +++ b/demos/demo_Riemannian_S3_vecTransp01.m @@ -3,7 +3,7 @@ function demo_Riemannian_S3_vecTransp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_SE2_GMM01.m b/demos/demo_Riemannian_SE2_GMM01.m index 35c864c..48a0e3f 100644 --- a/demos/demo_Riemannian_SE2_GMM01.m +++ b/demos/demo_Riemannian_SE2_GMM01.m @@ -4,7 +4,7 @@ function demo_Riemannian_SE2_GMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_SE2_interp01.m b/demos/demo_Riemannian_SE2_interp01.m index 3ec31e1..bb95e0e 100644 --- a/demos/demo_Riemannian_SE2_interp01.m +++ b/demos/demo_Riemannian_SE2_interp01.m @@ -4,7 +4,7 @@ function demo_Riemannian_SE2_interp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_SOd_interp01.m b/demos/demo_Riemannian_SOd_interp01.m index 9e08072..2f91b98 100644 --- a/demos/demo_Riemannian_SOd_interp01.m +++ b/demos/demo_Riemannian_SOd_interp01.m @@ -4,7 +4,7 @@ function demo_Riemannian_SOd_interp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_GMM01.m b/demos/demo_Riemannian_Sd_GMM01.m index 8345e99..85a41a3 100644 --- a/demos/demo_Riemannian_Sd_GMM01.m +++ b/demos/demo_Riemannian_Sd_GMM01.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_GMM02.m b/demos/demo_Riemannian_Sd_GMM02.m index 454e07d..7241b0d 100644 --- a/demos/demo_Riemannian_Sd_GMM02.m +++ b/demos/demo_Riemannian_Sd_GMM02.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMM02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_GMR01.m b/demos/demo_Riemannian_Sd_GMR01.m index e01b757..3018bd4 100644 --- a/demos/demo_Riemannian_Sd_GMR01.m +++ b/demos/demo_Riemannian_Sd_GMR01.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMR01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_GMR02.m b/demos/demo_Riemannian_Sd_GMR02.m index c3fd241..7b3ab37 100644 --- a/demos/demo_Riemannian_Sd_GMR02.m +++ b/demos/demo_Riemannian_Sd_GMR02.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMR02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_GaussProd01.m b/demos/demo_Riemannian_Sd_GaussProd01.m index 066ca3d..9333597 100644 --- a/demos/demo_Riemannian_Sd_GaussProd01.m +++ b/demos/demo_Riemannian_Sd_GaussProd01.m @@ -3,7 +3,7 @@ function demo_Riemannian_Sd_GaussProd01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_MPC01.m b/demos/demo_Riemannian_Sd_MPC01.m index 9c3957c..a004551 100644 --- a/demos/demo_Riemannian_Sd_MPC01.m +++ b/demos/demo_Riemannian_Sd_MPC01.m @@ -6,7 +6,7 @@ function demo_Riemannian_Sd_MPC01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_MPC_infHor01.m b/demos/demo_Riemannian_Sd_MPC_infHor01.m index 60eb3a7..95fe04e 100644 --- a/demos/demo_Riemannian_Sd_MPC_infHor01.m +++ b/demos/demo_Riemannian_Sd_MPC_infHor01.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_MPC_infHor01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_interp01.m b/demos/demo_Riemannian_Sd_interp01.m index 6858686..76f8432 100644 --- a/demos/demo_Riemannian_Sd_interp01.m +++ b/demos/demo_Riemannian_Sd_interp01.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_interp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_interp02.m b/demos/demo_Riemannian_Sd_interp02.m index 009cc81..662d1d8 100644 --- a/demos/demo_Riemannian_Sd_interp02.m +++ b/demos/demo_Riemannian_Sd_interp02.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_interp02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_vecTransp01.m b/demos/demo_Riemannian_Sd_vecTransp01.m index b644b78..eddd03c 100644 --- a/demos/demo_Riemannian_Sd_vecTransp01.m +++ b/demos/demo_Riemannian_Sd_vecTransp01.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_vecTransp01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_Sd_vecTransp02.m b/demos/demo_Riemannian_Sd_vecTransp02.m index f147448..751f6cf 100644 --- a/demos/demo_Riemannian_Sd_vecTransp02.m +++ b/demos/demo_Riemannian_Sd_vecTransp02.m @@ -4,7 +4,7 @@ function demo_Riemannian_Sd_vecTransp02 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_Riemannian_pose_GMM01.m b/demos/demo_Riemannian_pose_GMM01.m index d72dbfd..64866cf 100644 --- a/demos/demo_Riemannian_pose_GMM01.m +++ b/demos/demo_Riemannian_pose_GMM01.m @@ -3,7 +3,7 @@ function demo_Riemannian_pose_GMM01 % % If this code is useful for your research, please cite the related publication: % @article{Calinon20RAM, -% author="Calinon, S. and Jaquier, N.", +% author="Calinon, S.", % title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control", % journal="{IEEE} Robotics and Automation Magazine ({RAM})", % year="2020", diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m index a25a8d9..3a5902f 100644 --- a/demos/demo_ergodicControl_1D01.m +++ b/demos/demo_ergodicControl_1D01.m @@ -36,7 +36,7 @@ addpath('./m_fcts/'); %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -nbData = 4000; %Number of datapoints +nbData = 8000; %Number of datapoints nbFct = 50; %Number of basis functions along x and y nbStates = 2; %Number of Gaussians to represent the spatial distribution dt = 1E-2; %Time step @@ -44,7 +44,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)] om = 2 .* pi ./ L; %omega u_max = 1E0; %Maximum speed allowed -x = .1; %Initial position %Desired spatial distribution represented as a mixture of Gaussians Mu(:,1) = 0.7 *1; @@ -59,69 +58,161 @@ Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients rg = [0:nbFct-1]'; Lambda = (rg.^2 + 1).^-1; %Weighting vector (Eq.(15) -HK = L; %Rescaling term (as scalar) -% HK = [1; sqrt(.5)*ones(nbFct-1,1)]; %Rescaling term (as normalizing vector) - %Explicit description of w_hat by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries) kk = rg .* om; w_hat = zeros(nbFct,1); for j=1:nbStates w_hat = w_hat + Priors(j) .* cos(kk .* Mu(:,j)) .* exp(-.5 .* kk.^2 .* Sigma(:,j)); %Eq.(22) end -w_hat = w_hat ./ HK; +w_hat = w_hat ./ L; + +% %Alternative computation of w_hat by discretization (for verification) +% nbRes = 1000; +% x = linspace(xlim(1), xlim(2), nbRes); %Spatial range +% g = zeros(1,nbRes); +% for k=1:nbStates +% g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution +% end +% phi_inv = cos(rg * x .* om) ./ L ./ nbRes; +% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution +% +% % w_hat = zeros(nbFct,1); +% % for n=1:nbFct +% % w_hat(n) = sum(g .* cos(x .* rg(n) .* om)); +% % end +% % w_hat = w_hat ./ L ./ nbRes; %Fourier coefficients of spatial distribution + +%Fourier basis functions (for a discretized map) +nbRes = 100; +xm = linspace(xlim(1), xlim(2), nbRes); %Spatial range +phim = cos(rg * xm .* om) .* 2; %Fourier basis functions +phim(2:end,:) = phim(2:end,:) .* 2; + +%Desired spatial distribution +g = w_hat' * phim; + +% %Alternative computation of g +% g = zeros(1,nbRes); +% for k=1:nbStates +% g = g + Priors(k) .* mvnpdf(xm', Mu(:,k)', Sigma(:,k))'; %Spatial distribution +% %(2.*pi.*s1).^-.5 .* exp(-.5 .* s1.^-1 .* (X - m1).^2); +% end %% Ergodic control %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +x = .1; %Initial position wt = zeros(nbFct, 1); for t=1:nbData r.x(:,t) = x; %Log data %Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction) phi = cos(x * rg .* om); %Eq.(18) - dphi = -sin(x * rg .* om) .* rg .* om; - -% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) -% fx = real(exp(-i .* x * rg .* om)); %Eq.(18) -% gradf = real(-exp(-i .* x * rg .* om) .* i .* rg .* om); + dphi = -sin(x * rg .* om) .* rg .* om; %Gradient of basis functions - wt = wt + phi ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) + wt = wt + phi ./ L; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) % %Controller with ridge regression formulation -% u = -gradf' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command +% u = -dphi' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command %Controller with constrained velocity norm u = -dphi' * (Lambda .* (wt./t - w_hat)); %Eq.(24) u = u .* u_max ./ (norm(u)+1E-2); %Velocity command x = x + u .* dt; %Update of position + r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution +% r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation end -% % r.G = real(A * ck' / nbData); -% invA = exp(-Xm' * KX *i*2*pi) / nbData ./ repmat(HK,nbRes,1); -% r.G = real(invA * ck'); + +% %Alternative computation of desired and reconstructed spatial distribution +% g = zeros(1,nbRes); +% for n=1:nbFct +% g = g + w_hat(n) .* cos(xm .* rg(n) .* om); +% end +% r.g = zeros(1,nbRes); +% for n=1:nbFct +% r.g = r.g + (wt(n)./nbData) .* cos(xm .* rg(n) .* om); +% end -%% Plot +%% Plot (static) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -figure('position',[10,10,1800,900],'color',[1 1 1]); -%Plot signal -subplot(1,3,[1,2]); hold on; -plot(1:nbData, r.x, '-','linewidth',3,'color',[.2 .2 .2]); -axis([1, nbData, xlim']); -xlabel('t','fontsize',18); ylabel('x','fontsize',18); -set(gca,'xtick',[],'ytick',[]); +s = nbData; %Reconstruction at time step s +T = nbData; %Size of time window +figure('position',[10,10,1200,1200],'color',[1 1 1]); %Plot distribution -subplot(1,3,3); hold on; -nbRes = 100; -x = linspace(xlim(1),xlim(2),nbRes); -g = zeros(1,nbRes); -for k=1:nbStates - g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution -end -plot(g, x, '-','linewidth',3,'color',[.8 0 0]); -xlabel('\phi(x)','fontsize',18); ylabel('x','fontsize',18); -set(gca,'xtick',[],'ytick',[]); - +subplot(3,1,1); hold on; +h(1) = plot(xm, g, '-','linewidth',3,'color',[.8 0 0]); +h(2) = plot(xm, r.g(:,s), '-','linewidth',3,'color',[0 0 0]); +legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest'); +axis([xlim', -.3, max(g)*1.2]); +xlabel('$x$','interpreter','latex','fontsize',28); +ylabel('$g(x)$','interpreter','latex','fontsize',28); +set(gca,'linewidth',2,'xtick',[],'ytick',[]); +%Plot signal +subplot(3,1,[2,3]); hold on; box on; +plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]); +plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0]); +axis([xlim', 1, T]); +xlabel('$x$','interpreter','latex','fontsize',28); +set(gca,'linewidth',2,'xtick',[],'ytick',[1,T],'yticklabel',{'t-T','t'},'fontsize',24); % print('-dpng','graphs/ergodicControl_1D01.png'); + +% %Plot error +% figure; hold on; +% plot(r.e(200:end)); + +% %Plot decomposition of desired distribution +% dg = 4; +% figure('position',[10,10,900,1200],'color',[1 1 1]); hold on; axis off; +% %Plot distribution +% plot(xm, g*2, '-','linewidth',3,'color',[.8 0 0]); +% for n=1:6 +% plot(xm, .3*phim(n,:)-dg*n,'-','linewidth',2,'color',[0 .6 0]); +% end +% % print('-dpng','graphs/ergodicControl_1DbasisFcts01.png'); + + +% %% Plot (animated) +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% T = 2000; %Size of time window +% s0 = 1; %Animation from time step s0 +% figure('position',[10,10,1200,1200],'color',[1 1 1]); +% %Plot distribution +% subplot(3,1,1); hold on; +% h(1) = plot(xm, g, '-','linewidth',3,'color',[.8 0 0]); +% h(2) = plot(xm, r.g(:,s0), '-','linewidth',3,'color',[0 0 0]); +% axis([xlim', -.3, max(g)*1.2]); +% xlabel('$x$','interpreter','latex','fontsize',28); +% ylabel('$g(x)$','interpreter','latex','fontsize',28); +% set(gca,'linewidth',2,'xtick',[],'ytick',[]); +% %Plot signal +% subplot(3,1,[2,3]); hold on; box on; +% axis([xlim', 1, T]); +% xlabel('$x$','interpreter','latex','fontsize',28); +% set(gca,'linewidth',2,'xtick',[],'ytick',[1,T],'yticklabel',{'t-T','t'},'fontsize',24); +% %Animation +% hs=[]; id=1; +% for s = s0:500:nbData +% %Plot distribution +% subplot(3,1,1); hold on; +% delete(h(2)); +% h(2) = plot(xm, r.g(:,s), '-','linewidth',3,'color',[0 0 0]); +% legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest'); +% %Plot signal +% subplot(3,1,[2,3]); hold on; box on; +% delete(hs); +% if s>T +% hs(1) = plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]); +% else +% hs(1) = plot(r.x(:,1:s), T-s+1:T, '-','linewidth',3,'color',[0 0 0]); +% end +% hs(2) = plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0]); +% drawnow; +% % print('-dpng',['graphs/anim/ergodicControl_1Danim' num2str(id,'%.3d') '.png']); +% % id = id + 1; +% % pause(.1); +% end + pause; close all; \ No newline at end of file diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m index f662e14..0a6858c 100644 --- a/demos/demo_ergodicControl_2D01.m +++ b/demos/demo_ergodicControl_2D01.m @@ -36,7 +36,7 @@ addpath('./m_fcts/'); %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -nbData = 4000; %Number of datapoints +nbData = 8000; %Number of datapoints nbFct = 20; %Number of basis functions along x and y nbVar = 2; %Dimension of datapoint nbStates = 2; %Number of Gaussians to represent the spatial distribution @@ -45,8 +45,7 @@ dt = 1E-2; %Time step xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation) L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)] om = 2 .* pi ./ L; %omega -u_max = 5E0; %Maximum speed allowed -x = [.1; .3]; %Initial position +u_max = 5E1; %Maximum speed allowed %Desired spatial distribution represented as a mixture of Gaussians Mu(:,1) = [.3; .7]; @@ -63,10 +62,6 @@ rg = 0:nbFct-1; [KX(1,:,:), KX(2,:,:)] = ndgrid(rg, rg); Lambda = (KX(1,:).^2 + KX(2,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15)) -HK = L^nbVar; %Rescaling term (as scalar) -% hk = [1; sqrt(.5)*ones(nbFct-1,1)]; -% HK = hk(xx,1) .* hk(yy,1); %Rescaling term (as normalizing matrix) - %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries) %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1]) op = hadamard(2^(nbVar-1)); @@ -81,11 +76,46 @@ for j=1:nbStates w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21) end end -w_hat = w_hat ./ HK ./ size(op,2); +w_hat = w_hat ./ L^nbVar ./ size(op,2); + +% %Alternative computation of w_hat by discretization (for verification) +% nbRes = 100; +% xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D +% [xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range +% phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* 2^nbVar; %Fourier basis functions +% g = zeros(1,nbRes^nbVar); +% for k=1:nbStates +% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution +% end +% phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) ./ L^nbVar ./ nbRes^nbVar; +% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution + +%Fourier basis functions (for a discretized map) +nbRes = 40; +xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D +[xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range +phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* 2^nbVar; %Fourier basis functions +% % phim(2:end,:) = phim(2:end,:) .* 2; +% phim = phim .* 2^nbVar; +% phim(1:nbFct,:) = phim(1:nbFct,:) .* .5; +% phim(1:nbFct:end,:) = phim(1:nbFct:end,:) .* .5; +hk = [1; 2*ones(nbFct-1,1)]; +HK = hk(xx(:)) .* hk(yy(:)); +phim = phim .* repmat(HK,[1,nbRes^nbVar]); + +%Desired spatial distribution +g = w_hat' * phim; + +% %Alternative computation of g +% g = zeros(1,nbRes^nbVar); +% for k=1:nbStates +% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution +% end %% Ergodic control %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +x = [.1; .3]; %Initial position wt = zeros(nbFct^nbVar, 1); for t=1:nbData r.x(:,t) = x; %Log data @@ -94,12 +124,8 @@ for t=1:nbData phi1 = cos(x * rg .* om); %Eq.(18) dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om; -% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) -% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18) -% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om); - - dphi = [dphi1(1,xx) .* phi1(2,yy); phi1(1,xx) .* dphi1(2,yy)]; %gradient of basis functions - wt = wt + (phi1(1,xx) .* phi1(2,yy))' ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) + dphi = [dphi1(1,xx) .* phi1(2,yy); phi1(1,xx) .* dphi1(2,yy)]; %Gradient of basis functions + wt = wt + (phi1(1,xx) .* phi1(2,yy))' ./ L.^nbVar; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) % %Controller with ridge regression formulation % u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 5E-1; %Velocity command @@ -109,33 +135,69 @@ for t=1:nbData u = u .* u_max ./ (norm(u)+1E-1); %Velocity command x = x + u .* dt; %Update of position + r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution +% r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation end -%% Plot +%% Plot (static) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure('position',[10,10,1200,1200]); hold on; axis off; -plotGMM(Mu, Sigma, [.2 .2 .2], .3); +colormap(repmat(linspace(1,.4,64),3,1)'); +% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp'); +surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp'); +% plotGMM(Mu, Sigma, [.2 .2 .2], .3); plot(r.x(1,:), r.x(2,:), '-','linewidth',1,'color',[0 0 0]); plot(r.x(1,1), r.x(2,1), '.','markersize',15,'color',[0 0 0]); axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; % print('-dpng','graphs/ergodicControl_2D01.png'); -%Plot Fourier series coefficients -figure('position',[1220,10,1300,700]); -colormap(repmat(linspace(1,.4,64),3,1)'); -subplot(1,2,1); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20); -imagesc(reshape(w_hat,nbFct,nbFct)); -axis tight; axis equal; axis ij; -subplot(1,2,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20); -imagesc(reshape(wt./t,[nbFct,nbFct])); -axis tight; axis equal; axis ij; - -% %Plot Fourier series coefficients (alternative version) -% figure('position',[1220,10,1300,700]); hold on; axis off; +% %Plot g as image +% figure; hold on; +% imagesc(reshape(g,[nbRes,nbRes])'); +% axis tight; axis equal; %axis ij; + +% %Plot g as graph +% figure('position',[10,10,2300,1300]); +% subplot(1,2,1); hold on; rotate3d on; +% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp'); +% view(3); axis vis3d; +% subplot(1,2,2); hold on; rotate3d on; +% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp'); +% view(3); axis vis3d; + +% %Plot Fourier series coefficients +% figure('position',[1220,10,1300,700]); % colormap(repmat(linspace(1,.4,64),3,1)'); -% imagesc([w_hat, reshape(wt./t,[nbFct,nbFct])]); +% subplot(1,2,1); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20); +% imagesc(reshape(w_hat,nbFct,nbFct)); % axis tight; axis equal; axis ij; +% subplot(1,2,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20); +% imagesc(reshape(wt./t,[nbFct,nbFct])); +% axis tight; axis equal; axis ij; +% +% % %Plot Fourier series coefficients (alternative version) +% % figure('position',[1220,10,1300,700]); hold on; axis off; +% % colormap(repmat(linspace(1,.4,64),3,1)'); +% % imagesc([w_hat, reshape(wt./t,[nbFct,nbFct])]); +% % axis tight; axis equal; axis ij; + + +% %% Plot (animated) +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% figure('position',[10,10,1200,1200]); hold on; axis off; +% colormap(repmat(linspace(1,.4,64),3,1)'); +% plot(r.x(1,1), r.x(2,1), '.','markersize',15,'color',[0 0 0]); +% axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; +% h=[]; id=1; +% for t=1:10:nbData +% delete(h); +% h(1) = surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,t),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp'); +% h(2) = plot(r.x(1,1:t), r.x(2,1:t), '-','linewidth',1,'color',[0 0 0]); +% drawnow; +% % print('-dpng',['graphs/anim/ergodicControl_2Danim' num2str(id,'%.3d') '.png']); +% % id = id + 1; +% end pause; close all; \ No newline at end of file diff --git a/demos/demo_ergodicControl_3D01.m b/demos/demo_ergodicControl_3D01.m index 8865f5b..199e653 100644 --- a/demos/demo_ergodicControl_3D01.m +++ b/demos/demo_ergodicControl_3D01.m @@ -37,7 +37,7 @@ addpath('./m_fcts/'); %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% nbData = 4000; %Number of datapoints -nbFct = 10; %Number of basis functions along x and y +nbFct = 20; %Number of basis functions along x and y nbVar = 3; %Dimension of datapoint nbStates = 2; %Number of Gaussians to represent the spatial distribution sp = (nbVar + 1) / 2; %Sobolev norm parameter @@ -46,7 +46,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)] om = 2 .* pi ./ L; %omega u_max = 1E1; %Maximum speed allowed -x = [.7; .1; .5]; %Initial position %Desired spatial distribution represented as a mixture of Gaussians Mu(:,1) = [.4; .5; .7]; @@ -63,7 +62,7 @@ rg = 0:nbFct-1; [KX(1,:,:,:), KX(2,:,:,:), KX(3,:,:,:)] = ndgrid(rg, rg, rg); Lambda = (KX(1,:).^2 + KX(2,:).^2 + KX(3,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15)) -HK = L^nbVar; %Rescaling term (as scalar) +% HK = L^nbVar; %Rescaling term (as scalar) % hk = [1; sqrt(.5)*ones(nbFct-1,1)]; % HK = hk(xx,1) .* hk(yy,1) .* hk(zz,1); %Rescaling term (as normalizing matrix) @@ -81,11 +80,42 @@ for j=1:nbStates w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21) end end -w_hat = w_hat ./ HK ./ size(op,2); +w_hat = w_hat ./ L^nbVar ./ size(op,2); + +% %Alternative computation of w_hat by discretization (for verification) +% nbRes = 20; +% xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D +% [xm(1,:,:,:), xm(2,:,:,:), xm(3,:,:,:)] = ndgrid(xm1d, xm1d, xm1d); %Spatial range +% phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) .* 2^nbVar; %Fourier basis functions +% g = zeros(1,nbRes^nbVar); +% for k=1:nbStates +% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution +% end +% phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) ./ L^nbVar ./ nbRes^nbVar; +% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution + +%Fourier basis functions (for a discretized map) +nbRes = 20; +xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D +[xm(1,:,:,:), xm(2,:,:,:), xm(3,:,:,:)] = ndgrid(xm1d, xm1d, xm1d); %Spatial range +phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) .* 2^nbVar; %Fourier basis functions +hk = [1; 2*ones(nbFct-1,1)]; +HK = hk(xx(:)) .* hk(yy(:)) .* hk(zz(:)); +phim = phim .* repmat(HK,[1,nbRes^nbVar]); + +%Desired spatial distribution +g = w_hat' * phim; + +% %Alternative computation of g +% g = zeros(1,nbRes^nbVar); +% for k=1:nbStates +% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution +% end %% Ergodic control %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +x = [.7; .1; .5]; %Initial position wt = zeros(nbFct^nbVar, 1); for t=1:nbData r.x(:,t) = x; %Log data @@ -94,12 +124,8 @@ for t=1:nbData phi1 = cos(x * rg .* om); %Eq.(18) dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om; -% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) -% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18) -% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om); - - dphi = [dphi1(1,xx) .* phi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* dphi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* phi1(2,yy) .* dphi1(3,zz)]; %gradient of basis functions - wt = wt + (phi1(1,xx) .* phi1(2,yy) .* phi1(3,zz))' ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) + dphi = [dphi1(1,xx) .* phi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* dphi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* phi1(2,yy) .* dphi1(3,zz)]; %Gradient of basis functions + wt = wt + (phi1(1,xx) .* phi1(2,yy) .* phi1(3,zz))' ./ L^nbVar; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) % %Controller with ridge regression formulation % u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command @@ -109,18 +135,38 @@ for t=1:nbData u = u .* u_max ./ (norm(u)+1E-1); %Velocity command x = x + u .* dt; %Update of position + +% r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution +% r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation end %% Plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -figure('position',[10,10,1200,1200]); hold on; axis off; rotate3d on; +figure('position',[10,10,1200,1200]); hold on; axis on; box on; rotate3d on; +% colormap(repmat(linspace(1,0,64),3,1)'); +colormap hsv; + plotGMM3D(Mu, Sigma, [.2 .2 .2], .3, 2); + +% [x,y,z] = meshgrid(xm1d, xm1d, xm1d); +% v = reshape(g,[nbRes,nbRes,nbRes]); +% v = permute(v,[2,1,3]); +% v(v<1E-3) = nan; +% % pcolor3(x,y,z,v,'alpha',.1); +% % vol3d('XData',x,'YData',y,'ZData',z,'CData',v); +% h = slice(x, y, z, v, [], [], xlim(1):.02:xlim(2)); +% set(h,'edgecolor','none','facecolor','interp','facealpha','interp'); +% alpha('color'); +% % alphamap('rampdown') +% % alphamap('increase',.05) + plot3(Mu(1,:), Mu(2,:), Mu(3,:), '.','markersize',15,'color',[0 0 0]); plot3(r.x(1,:), r.x(2,:), r.x(3,:), '-','linewidth',1,'color',[0 0 0]); plot3(r.x(1,1), r.x(2,1), r.x(3,1), '.','markersize',15,'color',[0 0 0]); -axis([xlim(1),xlim(2),xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; axis vis3d; view(60,25); -% plot3Dframe(eye(3).*.3, ones(3,1).*.2); +plot3Dframe(eye(3).*.3, ones(3,1).*.2); +axis([xlim(1),xlim(2),xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; axis vis3d; view(50,20); +set(gca,'xtick',[],'ytick',[],'ztick',[]); % print('-dpng','graphs/ergodicControl_3D01.png'); pause; diff --git a/demos/demo_ergodicControl_nD01.m b/demos/demo_ergodicControl_nD01.m index 5ee4446..6fcbfbe 100644 --- a/demos/demo_ergodicControl_nD01.m +++ b/demos/demo_ergodicControl_nD01.m @@ -37,7 +37,7 @@ addpath('./m_fcts/'); %% Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% nbData = 2000; %Number of datapoints -nbFct = 8; %Number of basis functions along x and y +nbFct = 5; %Number of basis functions along x and y nbVar = 4; %Dimension of datapoint nbStates = 2; %Number of Gaussians to represent the spatial distribution sp = (nbVar + 1) / 2; %Sobolev norm parameter @@ -46,7 +46,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)] om = 2 .* pi ./ L; %omega u_max = 5E1; %Maximum speed allowed -x = rand(nbVar,1); %Initial position %Desired spatial distribution represented as a mixture of Gaussians Mu = rand(nbVar,nbStates); @@ -68,12 +67,12 @@ for n=1:nbVar end Lambda = (stmp + 1).^-sp; %Weighting vector (Eq.(15)) -HK = L^nbVar; %Rescaling term (as scalar) -% hk = [1; sqrt(.5)*ones(nbFct-1,1)]; -% HK = ones(nbFct^nbVar, 1); -% for n=1:nbVar -% HK = HK .* hk(arr(n).x(:),1); %Rescaling term (as normalizing matrix) -% end +% HK = L^nbVar; %Rescaling term (as scalar) +% % hk = [1; sqrt(.5)*ones(nbFct-1,1)]; +% % HK = ones(nbFct^nbVar, 1); +% % for n=1:nbVar +% % HK = HK .* hk(arr(n).x(:),1); %Rescaling term (as normalizing matrix) +% % end %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries) %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1]) @@ -92,11 +91,12 @@ for j=1:nbStates w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21) end end -w_hat = w_hat ./ HK ./ size(op,2); +w_hat = w_hat ./ L^nbVar ./ size(op,2); %% Ergodic control %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +x = rand(nbVar,1); %Initial position wt = zeros(nbFct^nbVar, 1); for t=1:nbData r.x(:,t) = x; %Log data @@ -105,10 +105,6 @@ for t=1:nbData phi1 = cos(x * rg .* om); %Eq.(18) dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om; -% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) -% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18) -% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om); - dphi = ones(nbVar, nbFct^nbVar); phi = ones(nbFct^nbVar, 1); for n=1:nbVar @@ -121,7 +117,7 @@ for t=1:nbData end phi = phi .* phi1(n,arr(n).x(:))'; end - wt = wt + phi ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) + wt = wt + phi ./ L^nbVar; %wt./t are the Fourier series coefficients along trajectory (Eq.(17)) % %Controller with ridge regression formulation % u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command -- GitLab