diff --git a/README.md b/README.md
index a61185d5ac02a488dd007bddcf79595f2051eb3a..cd0c8fac7c03ff47a6847454e58fa50ab5a5a402 100644
--- a/README.md
+++ b/README.md
@@ -277,7 +277,7 @@ 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. (2019). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. arXiv:1909.05946.
+[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).
</a><br>
[[pdf]](http://calinon.ch/papers/Calinon-arXiv2019.pdf)
[[bib]](http://calinon.ch/papers/Calinon-arXiv2019.bib)
diff --git a/demos/demo_IK02.m b/demos/demo_IK02.m
index 5f4b98acb48454c0cd5102b0c9b925873b950cf1..50ebc1ab410e284bd54fd525459ea140f231a450 100644
--- a/demos/demo_IK02.m
+++ b/demos/demo_IK02.m
@@ -103,20 +103,23 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('PaperPosition',[0 0 16 12],'position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
+figure('position',[10,10,2300,1200],'color',[1 1 1]); hold on; axis off;
listT = round(linspace(1,nbData,10));
i=0;
for t=listT
i=i+1;
colTmp = [.9,.9,.9] - [.7,.7,.7] * i/length(listT);
- plotArm(r.q(1:3,t), ones(3,1)*armLength, [0;0;t/nbData], .03, colTmp); %left arm
- plotArm(r.q([1,4:5],t), ones(3,1)*armLength, [0;0;t/nbData+0.1], .03, colTmp); %right arm
+ plotArm(r.q(1:3,t), ones(3,1)*armLength, [0;0;i], .03, colTmp); %left arm
+ plotArm(r.q([1,4:5],t), ones(3,1)*armLength, [0;0;i+0.1], .03, colTmp); %right arm
end
-plot3(rxh(1,:), rxh(2,:), ones(1,nbData)*2, 'r-','linewidth',2);
-plot3(lxh(1,:), lxh(2,:), ones(1,nbData)*2, 'r-','linewidth',2);
+plot3(rxh(1,:), rxh(2,:), ones(1,nbData)*20, '-','linewidth',2,'color',[.8 0 0]);
+plot3(lxh(1,:), lxh(2,:), ones(1,nbData)*20, '-','linewidth',2,'color',[.8 0 0]);
+plot3(rxh(1,listT), rxh(2,listT), ones(1,10)*20, '.','markersize',25,'color',[.8 0 0]);
+plot3(lxh(1,listT), lxh(2,listT), ones(1,10)*20, '.','markersize',25,'color',[.8 0 0]);
+
%axis([-.2 1 -.2 1]);
axis equal; axis tight;
-%print('-dpng','graphs/demoIK02.png');
+% print('-dpng','graphs/demoIK02.png');
pause;
close all;
\ No newline at end of file
diff --git a/demos/demo_Riemannian_Gdp_vecTransp01.m b/demos/demo_Riemannian_Gdp_vecTransp01.m
index f17952be156c5b35d67d42712a27dd958e37c2e1..6120e820ee8025973d540510d024dadda410347e 100644
--- a/demos/demo_Riemannian_Gdp_vecTransp01.m
+++ b/demos/demo_Riemannian_Gdp_vecTransp01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_Gdp_vecTransp01
% Parallel transport on the Grassmann manifold G(3,2)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Hd_GMM01.m b/demos/demo_Riemannian_Hd_GMM01.m
index 147d06897f95856411a45fc8cec59b6cb309b390..9775aff6151ea8d88eee0327517a286baba7b339 100644
--- a/demos/demo_Riemannian_Hd_GMM01.m
+++ b/demos/demo_Riemannian_Hd_GMM01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_Hd_GMM01
% GMM on n-hyperboloid manifold
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Hd_interp01.m b/demos/demo_Riemannian_Hd_interp01.m
index 556303e597c2640ee7f7186f32b88f5a4484286f..c42a16dfe7f3438555802d91e3696bd5f7bc90ba 100644
--- a/demos/demo_Riemannian_Hd_interp01.m
+++ b/demos/demo_Riemannian_Hd_interp01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_Hd_interp01
% Interpolation on n-hyperboloid manifold
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S1_interp01.m b/demos/demo_Riemannian_S1_interp01.m
index efb6cc227653cb6055ee0b470a9089caa412ef69..fe8d1cbae070edac140bff7dd4007b78bc133d09 100644
--- a/demos/demo_Riemannian_S1_interp01.m
+++ b/demos/demo_Riemannian_S1_interp01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S1_interp01
% Interpolation on 1-sphere manifold (formulation with imaginary numbers)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S1_interp02.m b/demos/demo_Riemannian_S1_interp02.m
index 20ad2334c09c5bf0dd5b520358782d9605e05030..81cdbfc8b4cb6b1ca0cfb6a05eda8912692b4cf8 100644
--- a/demos/demo_Riemannian_S1_interp02.m
+++ b/demos/demo_Riemannian_S1_interp02.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S1_interp02
% Interpolation on 1-sphere manifold (formulation with imaginary numbers, parameterization of x as angle)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_GMM01.m b/demos/demo_Riemannian_S2_GMM01.m
index 9aba5000113ad6ef856930748f83526cc188e21f..2943cd291ac60ed928ceec21e37d8fce781d84cd 100644
--- a/demos/demo_Riemannian_S2_GMM01.m
+++ b/demos/demo_Riemannian_S2_GMM01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_GMM01
% GMM for data on a sphere by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_GMR01.m b/demos/demo_Riemannian_S2_GMR01.m
index b060fa51fa739590539efc22ebb18cba542e7c41..a9e0aba14bd697a2081c5bb318a34b670e71c551 100644
--- a/demos/demo_Riemannian_S2_GMR01.m
+++ b/demos/demo_Riemannian_S2_GMR01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_GMR01
% GMR with input and output data on a sphere by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_GMR02.m b/demos/demo_Riemannian_S2_GMR02.m
index 6d61ea79665ec1bf9aad7de0fc28a35b697d636e..44d70eb2e4c25d2143418b83082a5fb5c2a0f32a 100644
--- a/demos/demo_Riemannian_S2_GMR02.m
+++ b/demos/demo_Riemannian_S2_GMR02.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_GMR02
% GMR with time as input and spherical data as output by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_GMR03.m b/demos/demo_Riemannian_S2_GMR03.m
index ae0efa06d6b88d10be4970eff8b0ac4493738e37..f676538ff6f48551f34759cea56113114042608e 100644
--- a/demos/demo_Riemannian_S2_GMR03.m
+++ b/demos/demo_Riemannian_S2_GMR03.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_GMR03
% GMR with 3D Euclidean data as input and spherical data as output by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_GMR04.m b/demos/demo_Riemannian_S2_GMR04.m
index 02dbebaa126f2c3d23f44d639b0bd7ecddfe2b4d..b40e14fb524aa748d2ad43974ecdd0e0f3c69631 100644
--- a/demos/demo_Riemannian_S2_GMR04.m
+++ b/demos/demo_Riemannian_S2_GMR04.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_GMR04
% GMR with input data on a sphere and output data in Euclidean space by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_GaussProd01.m b/demos/demo_Riemannian_S2_GaussProd01.m
index 9f0e5385e891feb8bf286b8abb68192d23344ed5..33bfa7406c91d4d7f971c731d9d7449bb9840019 100644
--- a/demos/demo_Riemannian_S2_GaussProd01.m
+++ b/demos/demo_Riemannian_S2_GaussProd01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_GaussProd01
% Product of Gaussians on a sphere by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_TPGMM01.m b/demos/demo_Riemannian_S2_TPGMM01.m
index b97d6c3dc8bb42507f7cf65d67232aab96f2c3ba..98d505708391307e1495e9a921667b2ed1c03312 100644
--- a/demos/demo_Riemannian_S2_TPGMM01.m
+++ b/demos/demo_Riemannian_S2_TPGMM01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_TPGMM01
% TP-GMM for data on a sphere by relying on Riemannian manifold (with single coordinate system).
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_TPGMM02.m b/demos/demo_Riemannian_S2_TPGMM02.m
index ee2f42d1cdfdeade50ced656101cf232a661e27f..58e707e54be57becead48f08e73c8281a6213d40 100644
--- a/demos/demo_Riemannian_S2_TPGMM02.m
+++ b/demos/demo_Riemannian_S2_TPGMM02.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_TPGMM02
% TP-GMM for data on a sphere by relying on Riemannian manifold (with two coordinate systems).
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -285,7 +285,7 @@ for n=1:nbRepros
end
%Manifold plot
-figure('PaperPosition',[0 0 18 6],'position',[10,10,1350,450]);
+figure('position',[10,10,2300,900]); %'PaperPosition',[0 0 18 6],
for n=1:nbRepros
subplot(1,3,n); hold on; axis off; rotate3d on;
colormap([.7 .7 .7]);
@@ -330,29 +330,30 @@ end
for m=1:model.nbFrames
for n=1:nbRepros
subplot(1,3,n); hold on;
- plot3(s(n).p(m).b(1), s(n).p(m).b(2), s(n).p(m).b(3), '.','markersize',22,'color',[.8 0 0]);
+ s(n).p(m).b = s(n).p(m).b .* 1.05;
+ plot3(s(n).p(m).b(1), s(n).p(m).b(2), s(n).p(m).b(3), '.','markersize',40,'color',[.8 0 0]);
% for nb=1:nbSamples
% plot3(squeeze(s(n).x(1,m,(nb-1)*nbData+1:nb*nbData)), squeeze(s(n).x(2,m,(nb-1)*nbData+1:nb*nbData)), squeeze(s(n).x(3,m,(nb-1)*nbData+1:nb*nbData)), '-','linewidth',.5,'color',min(clrmap(m,:)*1.5,1));
% end
% plot3(squeeze(s(n).xhat(1,m,:)), squeeze(s(n).xhat(2,m,:)), squeeze(s(n).xhat(3,m,:)), '.','markersize',12,'color',clrmap(m,:));
- clrmap(m,:)
- for t=1:nbData
+ for t=2:nbData
plot3(s(n).Gdisp(1,:,m,t), s(n).Gdisp(2,:,m,t), s(n).Gdisp(3,:,m,t), '-','linewidth',1,'color',clrmap(m,:)); %Transportation?
end
- view(-20,70); axis equal; axis tight; axis vis3d;
+ view(-20,70); axis vis3d; axis tight;
end
% print('-dpng','-r300',['graphs/demo_Riemannian_sphere_TPGMM' num2str(m) '.png']);
end
for n=1:nbRepros
subplot(1,3,n); hold on;
- plot3(s(n).xh(1,:), s(n).xh(2,:), s(n).xh(3,:), '.','markersize',8,'color',[0 0 0]);
- for t=1:nbData
+% plot3(s(n).xh(1,:), s(n).xh(2,:), s(n).xh(3,:), '.','markersize',8,'color',[0 0 0]);
+ plot3(s(n).xh(1,:), s(n).xh(2,:), s(n).xh(3,:), '-','linewidth',3,'color',[0 0 0]);
+ for t=2:nbData
plot3(s(n).Gh(1,:,t), s(n).Gh(2,:,t), s(n).Gh(3,:,t), '-','linewidth',2,'color',[0 0 0]);
end
end %n
-% print('-dpng','-r300','graphs/demo_Riemannian_sphere_TPGMM3.png');
+% print('-dpng','-r300','graphs/demo_Riemannian_S2_TPGMM02.png');
pause;
close all;
end
diff --git a/demos/demo_Riemannian_S2_batchLQR01.m b/demos/demo_Riemannian_S2_batchLQR01.m
index 74725f2d5a028650c60cfaf788d2812504e72b42..1fc96ecd430a93468e7ab7a282e322cce460144c 100644
--- a/demos/demo_Riemannian_S2_batchLQR01.m
+++ b/demos/demo_Riemannian_S2_batchLQR01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_S2_batchLQR01
% by using position and velocity data (-> acceleration commands)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_batchLQR02.m b/demos/demo_Riemannian_S2_batchLQR02.m
index 91e4ca7088c296bfca9675d9863d85ba7a0dbdea..5601c2067ba0f4fc8413f37f87409aaf0fa0bf03 100644
--- a/demos/demo_Riemannian_S2_batchLQR02.m
+++ b/demos/demo_Riemannian_S2_batchLQR02.m
@@ -3,12 +3,12 @@ function demo_Riemannian_S2_batchLQR02
% by using position and velocity data (-> acceleration commands)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_batchLQR03.m b/demos/demo_Riemannian_S2_batchLQR03.m
index ca7897fb5a45b2fb6da0ed7d86a99b3396415406..cd1ec900bcda741980d67b9a8993328686379b88 100644
--- a/demos/demo_Riemannian_S2_batchLQR03.m
+++ b/demos/demo_Riemannian_S2_batchLQR03.m
@@ -3,12 +3,12 @@ function demo_Riemannian_S2_batchLQR03
% by using only position data (-> velocity commands)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
index ca616741bee293af4148c5b8d17928217b7bc11b..bfe5cb1e3ab9d756ec0812917ba1be2e5cefe827 100644
--- a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
+++ b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_batchLQR_Bezier01
% Bezier interpolation on a sphere by relying on batch LQR with Riemannian manifold
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_infHorLQR01.m b/demos/demo_Riemannian_S2_infHorLQR01.m
index 22f37057a79934670ac484818abea96995f9cef8..0e3b1f816efd8ae9ce617b7672a7254e6b8217d0 100644
--- a/demos/demo_Riemannian_S2_infHorLQR01.m
+++ b/demos/demo_Riemannian_S2_infHorLQR01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_infHorLQR01
% Infinite-horizon linear quadratic regulation on a sphere by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S2_vecTransp01.m b/demos/demo_Riemannian_S2_vecTransp01.m
index 7d4e35a1bb09b47a59ce55b395498cf4dc51c58a..8fbc5b1818dff22a517e7f698d69e6103eee445f 100644
--- a/demos/demo_Riemannian_S2_vecTransp01.m
+++ b/demos/demo_Riemannian_S2_vecTransp01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S2_vecTransp01
% Parallel transport on a sphere.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S3_GMM01.m b/demos/demo_Riemannian_S3_GMM01.m
index 5f489f6c2489b9cad1aae339a518745cda09a491..3f4f063f138799dca08a438fd748749d1c70bd6e 100644
--- a/demos/demo_Riemannian_S3_GMM01.m
+++ b/demos/demo_Riemannian_S3_GMM01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S3_GMM01
% GMM for orientation data as unit quaternions by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S3_GMR01.m b/demos/demo_Riemannian_S3_GMR01.m
index c93200859a807ecf492a0227879a582acf0c37a0..e6bebee7311423f4a9f8c23b316d48830e4e39fe 100644
--- a/demos/demo_Riemannian_S3_GMR01.m
+++ b/demos/demo_Riemannian_S3_GMR01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S3_GMR01
% GMR with unit quaternions (orientations) as input and output data by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S3_GMR02.m b/demos/demo_Riemannian_S3_GMR02.m
index 44c5257a5842e0b9cc18c7da0adfdd2138b5acef..6f9943ee1ceddb9fae8dd6c3fe596dcaf2327754 100644
--- a/demos/demo_Riemannian_S3_GMR02.m
+++ b/demos/demo_Riemannian_S3_GMR02.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S3_GMR02
% GMR with time as input and unit quaternion (orientation) as output by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S3_infHorLQR01.m b/demos/demo_Riemannian_S3_infHorLQR01.m
index c3f2521a8e5af9d288a9e0dfd4cdf0f6d05424ee..af84917957604970599de35af532cea123c4d295 100644
--- a/demos/demo_Riemannian_S3_infHorLQR01.m
+++ b/demos/demo_Riemannian_S3_infHorLQR01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S3_infHorLQR01
% Linear quadratic regulation of unit quaternions (orientation) by relying on Riemannian manifold and infinite-horizon LQR
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_S3_interp01.m b/demos/demo_Riemannian_S3_interp01.m
index d7429bb9c82bd1b039cd031f9768dfe2d5d10614..5a8117c8bf993c5ea2ab2e5f9a6291478b243748 100644
--- a/demos/demo_Riemannian_S3_interp01.m
+++ b/demos/demo_Riemannian_S3_interp01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S3_interp01
% Interpolation of unit quaternions (orientation) by relying on Riemannian manifold, providing the same result as SLERP interpolation
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -32,14 +32,14 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbVar = 3; %Dimension of the tangent space
-model.nbVarMan = 4; %Dimension of the manifold
-model.nbStates = 2; %Number of states
+nbVar = 3; %Dimension of the tangent space
+nbVarMan = 4; %Dimension of the manifold
+nbStates = 2; %Number of states
nbData = 100; %Number of interpolation steps
% nbIter = 20; %Number of iteration for the Gauss Newton algorithm
-x = rand(model.nbVarMan,model.nbStates) - 0.5;
-for i=1:model.nbStates
+x = rand(nbVarMan,nbStates) - 0.5;
+for i=1:nbStates
x(:,i) = x(:,i) / norm(x(:,i));
end
@@ -47,14 +47,14 @@ end
%% Geodesic interpolation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w = [linspace(1,0,nbData); linspace(0,1,nbData)];
-xi = zeros(model.nbVarMan,nbData);
+xi = zeros(nbVarMan,nbData);
% xtmp = x(:,1);
for t=1:nbData
% %Interpolation between more than 2 points can be computed in an iterative form
% for n=1:nbIter
-% utmp = zeros(model.nbVar,1);
-% for i=1:model.nbStates
+% utmp = zeros(nbVar,1);
+% for i=1:nbStates
% utmp = utmp + w(i,t) * logmap(x(:,i), xtmp);
% end
% xtmp = expmap(utmp, xtmp);
@@ -73,25 +73,88 @@ for t=1:nbData
end
+% %% Naive interpolation
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% R1 = quat2rotm(x(:,1)');
+% R2 = quat2rotm(x(:,2)');
+% for t=1:nbData
+% [U,~,V] = svd(w(1,t) .* R1 + w(2,t) .* R2);
+% R = U * V';
+% xi2(:,t) = rotm2quat(R);
+% end
+
+
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,900,1250]);
-for i=1:model.nbVarMan
- subplot(model.nbVarMan,1,i); hold on;
- for n=1:model.nbStates
+for i=1:nbVarMan
+ subplot(nbVarMan,1,i); hold on;
+ for n=1:nbStates
plot([1,nbData],[x(i,n),x(i,n)],'-','linewidth',2,'color',[0 0 0]);
plot([1,nbData],[-x(i,n),-x(i,n)],'--','linewidth',2,'color',[0 0 0]);
end
h(1) = plot(xi(i,:),'-','linewidth',2,'color',[.8 0 0]);
h(2) = plot(xi2(i,:),':','linewidth',2,'color',[0 .7 0]);
if i==1
- legend(h,'geodesic','SLERP');
+% legend(h,'geodesic','SLERP');
+ legend(h,'geodesic','Naive interpolation');
end
ylabel(['q_' num2str(i)]);
axis([1, nbData -1 1]);
end
xlabel('t');
+%3D plot
+figure('position',[920,10,900,1250]);
+subplot(2,1,1); hold on; axis off; rotate3d on;
+colormap([.9 .9 .9]);
+[X,Y,Z] = sphere(20);
+mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
+plot3Dframe(quat2rotm(x(:,end)'), zeros(3,1), eye(3)*.3);
+view(3); axis equal; axis tight; axis vis3d;
+h=[];
+for t=1:nbData
+ if mod(t,10)~=0
+ delete(h);
+ end
+ h = plot3Dframe(quat2rotm(xi(:,t)'), zeros(3,1));
+ drawnow;
+end
+
+%SLERP interpolation
+subplot(2,1,2); hold on; axis off; rotate3d on;
+colormap([.9 .9 .9]);
+[X,Y,Z] = sphere(20);
+mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
+plot3Dframe(quat2rotm(x(:,end)'), zeros(3,1), eye(3)*.3);
+view(3); axis equal; axis tight; axis vis3d;
+h=[];
+for t=1:nbData
+ if mod(t,10)~=0
+ delete(h);
+ end
+ h = plot3Dframe(quat2rotm(xi2(:,t)'), zeros(3,1));
+ drawnow;
+end
+
+% %Naive interpolation
+% subplot(2,1,2); hold on; axis off; rotate3d on;
+% colormap([.9 .9 .9]);
+% [X,Y,Z] = sphere(20);
+% mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
+% plot3Dframe(quat2rotm(x(:,end)'), zeros(3,1), eye(3)*.3);
+% view(3); axis equal; axis tight; axis vis3d;
+% h=[];
+% for t=1:nbData
+% if mod(t,10)~=0
+% delete(h);
+% end
+% [U,~,V] = svd(w(1,t) .* R1 + w(2,t) .* R2);
+% R = U * V';
+% h = plot3Dframe(R, zeros(3,1));
+% drawnow;
+% end
+
%print('-dpng','graphs/demo_Riemannian_S3_interp01.png');
pause;
close all;
diff --git a/demos/demo_Riemannian_S3_vecTransp01.m b/demos/demo_Riemannian_S3_vecTransp01.m
index 10dedf539f92a05ee7f6cbb6563c5506c86579d7..68a1e656b058de18f7115148d9ab637f630a9709 100644
--- a/demos/demo_Riemannian_S3_vecTransp01.m
+++ b/demos/demo_Riemannian_S3_vecTransp01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_S3_vecTransp01
% Parallel transport for unit quaternions (orientation).
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_SE2_GMM01.m b/demos/demo_Riemannian_SE2_GMM01.m
index 52eafde5d83875727f998470fbc64fd33a193db6..35c864c9a65a5c203bfbaa46ed83426eb0ce9bd4 100644
--- a/demos/demo_Riemannian_SE2_GMM01.m
+++ b/demos/demo_Riemannian_SE2_GMM01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_SE2_GMM01
% (Implementation of exp and log maps based on "Lie Groups for 2D and 3D Transformations" by Ethan Eade)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_SE2_interp01.m b/demos/demo_Riemannian_SE2_interp01.m
index 54eacc18569fa1b0910cd74301061f6d023bdff0..3ec31e162574598e085a49844c9fb367a7326c63 100644
--- a/demos/demo_Riemannian_SE2_interp01.m
+++ b/demos/demo_Riemannian_SE2_interp01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_SE2_interp01
% (Implementation of exp and log maps based on "Lie Groups for 2D and 3D Transformations" by Ethan Eade)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_SOd_interp01.m b/demos/demo_Riemannian_SOd_interp01.m
index 2a6f0ea6e0451e62c3b694f5d587a43098ec1e83..9e080720b2d7f0dc83584fea53bbc0c814f0c43a 100644
--- a/demos/demo_Riemannian_SOd_interp01.m
+++ b/demos/demo_Riemannian_SOd_interp01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_SOd_interp01
% (implementation of exp and log maps based on "Newton method for Riemannian centroid computation in naturally reductive homogeneous spaces")
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Sd_GMM01.m b/demos/demo_Riemannian_Sd_GMM01.m
index 131f2465bc06ef57d5486086aba00136cfd9f3ec..8345e9904785fc5806419a87ac0bd9f733594f43 100644
--- a/demos/demo_Riemannian_Sd_GMM01.m
+++ b/demos/demo_Riemannian_Sd_GMM01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_GMM01
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -96,6 +96,7 @@ tl = linspace(-pi, pi, nbDrawingSeg);
%Computation of covariance contours on the circle
Gdisp = zeros(model.nbVar, nbDrawingSeg, model.nbStates);
Gdisp2 = zeros(model.nbVar, nbDrawingSeg, model.nbStates);
+
for i=1:model.nbStates
[V,D] = eig(model.Sigma(:,:,i));
Gdisp(:,:,i) = expmap(real(V*D.^.5)*[cos(tl); sin(tl)], model.MuMan(:,i));
diff --git a/demos/demo_Riemannian_Sd_GMM02.m b/demos/demo_Riemannian_Sd_GMM02.m
index f597f3030d1806474d93239113ce14b22d475e58..454e07d3e656fc954c72379bb716defaeba8890c 100644
--- a/demos/demo_Riemannian_Sd_GMM02.m
+++ b/demos/demo_Riemannian_Sd_GMM02.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_GMM02
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -145,6 +145,9 @@ end
function x = expmap(u, x0)
theta = sqrt(sum(u.^2,1));
x = real(repmat(x0,[1,size(u,2)]) .* repmat(cos(theta),[size(u,1),1]) + u .* repmat(sin(theta)./theta,[size(u,1),1]));
+ for t=1:size(x,2)
+ norm(x(:,t))
+ end
x(:,theta<1e-16) = repmat(x0,[1,sum(theta<1e-16)]);
end
diff --git a/demos/demo_Riemannian_Sd_GMR01.m b/demos/demo_Riemannian_Sd_GMR01.m
index 72726af069934a4e137730260fc6b4604f6a3da6..e01b7577824e9d74cdd8a890547dd1c15e5716af 100644
--- a/demos/demo_Riemannian_Sd_GMR01.m
+++ b/demos/demo_Riemannian_Sd_GMR01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_GMR01
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Sd_GMR02.m b/demos/demo_Riemannian_Sd_GMR02.m
index fe48c56325cfa85b1054c8465dc4f619b498f576..c3fd241b70d622aed8e5d7aac87c4efa4de33cb8 100644
--- a/demos/demo_Riemannian_Sd_GMR02.m
+++ b/demos/demo_Riemannian_Sd_GMR02.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_GMR02
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Sd_GaussProd01.m b/demos/demo_Riemannian_Sd_GaussProd01.m
index 6f54cdb6724f5c9bb9898f14ed9d19a99bffbb5d..066ca3d1d6fbfe1225aff73733b2ca0aeff9c334 100644
--- a/demos/demo_Riemannian_Sd_GaussProd01.m
+++ b/demos/demo_Riemannian_Sd_GaussProd01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_Sd_GaussProd01
% Product of Gaussians on a d-sphere by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Sd_MPC01.m b/demos/demo_Riemannian_Sd_MPC01.m
index cdf59235b213e46cb42bb91a920488381c777e11..9c3957cff8de68f9ec9751eaa43413b39a4fedf2 100644
--- a/demos/demo_Riemannian_Sd_MPC01.m
+++ b/demos/demo_Riemannian_Sd_MPC01.m
@@ -5,12 +5,12 @@ function demo_Riemannian_Sd_MPC01
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Sd_MPC_infHor01.m b/demos/demo_Riemannian_Sd_MPC_infHor01.m
index 808e4b39bf6251be13e5da7a5535ca2b7c407970..60eb3a73459315af79dfcf32c8b81a6315afc2a5 100644
--- a/demos/demo_Riemannian_Sd_MPC_infHor01.m
+++ b/demos/demo_Riemannian_Sd_MPC_infHor01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_MPC_infHor01
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -128,7 +128,7 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Timeline plot
-figure('PaperPosition',[0 0 6 8],'position',[10,10,650,650],'name','timeline data');
+figure('PaperPosition',[0 0 6 8],'position',[10,10,650,650]);
for k=1:4
subplot(2,2,k); hold on;
for n=1:nbRepros
@@ -141,7 +141,7 @@ for k=1:4
end
%3D plot
-figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650],'name','timeline data'); hold on; axis off; rotate3d on;
+figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650]); hold on; axis off; rotate3d on;
colormap([.9 .9 .9]);
[X,Y,Z] = sphere(20);
mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
diff --git a/demos/demo_Riemannian_Sd_interp01.m b/demos/demo_Riemannian_Sd_interp01.m
index 2abcd28d932196fd79752c349205f184eb55bfba..68586869c73daf67d8ac4b06cb05df6dda3a1d0f 100644
--- a/demos/demo_Riemannian_Sd_interp01.m
+++ b/demos/demo_Riemannian_Sd_interp01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_interp01
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -117,7 +117,10 @@ end
function x = expmap(u,x0)
th = sqrt(sum(u.^2,1)); %norm(u,'fro')
x = repmat(x0,[1,size(u,2)]) .* repmat(cos(th),[size(u,1),1]) + u .* repmat(sin(th)./th,[size(u,1),1]);
- x(:,th<1e-16) = repmat(x0,[1,sum(th<1e-16)]);
+ x(:,th<1e-16) = repmat(x0,[1,sum(th<1e-16)]);
+% for t=1:size(x,2)
+% norm(x(:,t))
+% end
end
function u = logmap(x,x0)
diff --git a/demos/demo_Riemannian_Sd_interp02.m b/demos/demo_Riemannian_Sd_interp02.m
index dfeee925f2c0217af52dbabf56e023676c00dea6..009cc81ea52e09b52fd6dff9672edde644cea813 100644
--- a/demos/demo_Riemannian_Sd_interp02.m
+++ b/demos/demo_Riemannian_Sd_interp02.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_interp02
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -33,13 +33,13 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbVar = 4; %Number of variables
-model.nbStates = 2; %Number of states
+nbVar = 4; %Number of variables
+nbStates = 2; %Number of states
nbData = 100; %Number of interpolation steps
% nbIter = 20; %Number of iteration for the Gauss Newton algorithm
-x = rand(model.nbVar,model.nbStates) - 0.5;
-for i=1:model.nbStates
+x = rand(nbVar,nbStates) - 0.5;
+for i=1:nbStates
x(:,i) = x(:,i) / norm(x(:,i));
end
@@ -47,14 +47,14 @@ end
%% Geodesic interpolation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w = [linspace(1,0,nbData); linspace(0,1,nbData)];
-xi = zeros(model.nbVar,nbData);
+xi = zeros(nbVar,nbData);
% xtmp = x(:,1);
for t=1:nbData
% %Interpolation between more than 2 points can be computed in an iterative form
% for n=1:nbIter
-% utmp = zeros(model.nbVar,1);
-% for i=1:model.nbStates
+% utmp = zeros(nbVar,1);
+% for i=1:nbStates
% utmp = utmp + w(i,t) * logmap(x(:,i), xtmp);
% end
% xtmp = expmap(utmp, xtmp);
@@ -76,9 +76,9 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,900,1250]);
-for i=1:model.nbVar
- subplot(model.nbVar,1,i); hold on;
- for n=1:model.nbStates
+for i=1:nbVar
+ subplot(nbVar,1,i); hold on;
+ for n=1:nbStates
plot([1,nbData],[x(i,n),x(i,n)],'-','linewidth',2,'color',[0 0 0]);
plot([1,nbData],[-x(i,n),-x(i,n)],'--','linewidth',2,'color',[0 0 0]);
end
@@ -92,6 +92,27 @@ for i=1:model.nbVar
end
xlabel('t');
+%3D plot
+figure('PaperPosition',[0 0 6 8],'position',[910,10,650,650]); hold on; axis off; rotate3d on;
+colormap([.9 .9 .9]);
+[X,Y,Z] = sphere(20);
+mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
+plot3Dframe(quat2rotm(x(:,end)'), zeros(3,1), eye(3)*.3);
+view(3); axis equal; axis tight; axis vis3d;
+h=[];
+for t=1:nbData
+ delete(h);
+ h = plot3Dframe(quat2rotm(xi(:,t)'), zeros(3,1));
+ drawnow;
+end
+
+pause;
+for t=1:nbData
+ delete(h);
+ h = plot3Dframe(quat2rotm(xi2(:,t)'), zeros(3,1));
+ drawnow;
+end
+
%print('-dpng','graphs/demo_Riemannian_Sd_interp02.png');
pause;
close all;
diff --git a/demos/demo_Riemannian_Sd_vecTransp01.m b/demos/demo_Riemannian_Sd_vecTransp01.m
index d6a0793aa691a984e7e67d08e1a7e4d63111f8f0..b644b7863156337953d15fbeeef94c368cc24620 100644
--- a/demos/demo_Riemannian_Sd_vecTransp01.m
+++ b/demos/demo_Riemannian_Sd_vecTransp01.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_vecTransp01
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_Sd_vecTransp02.m b/demos/demo_Riemannian_Sd_vecTransp02.m
index 40d0b3f827c04b17c91798161df1af6ded321716..f1474483f94f74ab28b110627e2f27d9e3d2bcca 100644
--- a/demos/demo_Riemannian_Sd_vecTransp02.m
+++ b/demos/demo_Riemannian_Sd_vecTransp02.m
@@ -3,12 +3,12 @@ function demo_Riemannian_Sd_vecTransp02
% (formulation with tangent space of the same dimension as the dimension of the manifold)
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_Riemannian_pose_GMM01.m b/demos/demo_Riemannian_pose_GMM01.m
index 28b3be50b8c781136f20cebdd375a35bf1ff88ea..d72dbfd4520b956713d369c8de1a361268793d91 100644
--- a/demos/demo_Riemannian_pose_GMM01.m
+++ b/demos/demo_Riemannian_pose_GMM01.m
@@ -2,12 +2,12 @@ function demo_Riemannian_pose_GMM01
% GMM to encode 3D position and orientation as unit quaternion by relying on Riemannian manifold.
%
% If this code is useful for your research, please cite the related publication:
-% @article{Calinon19,
+% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
-% journal="arXiv:1909.05946",
-% year="2019",
-% pages="1--10"
+% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
+% year="2020",
+% pages=""
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_TPGMM01.m b/demos/demo_TPGMM01.m
index 5384b13b297b964b8080e0f1c4785d93e2f4331d..582367767d25b03b8b4ee33aa56cc8876ff04e14 100644
--- a/demos/demo_TPGMM01.m
+++ b/demos/demo_TPGMM01.m
@@ -163,7 +163,7 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
- colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
+ colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
end
if ~exist('fa')
fa = .6;
diff --git a/demos/demo_TPGMM_teleoperation01.m b/demos/demo_TPGMM_teleoperation01.m
index 243a9d1c0f7dfd1e38262bcba194dc36557ed902..2ca89993bfa68dab87cdc67e0dbb42bb044fb79e 100644
--- a/demos/demo_TPGMM_teleoperation01.m
+++ b/demos/demo_TPGMM_teleoperation01.m
@@ -43,28 +43,45 @@ model.nbFrames = 1; %Number of candidate frames of reference
model.nbVarIn = 2; %Input dimension (position of object)
model.nbVarOut = 1; %Output dimension (orientation of robot end-effector)
model.nbVar = model.nbVarIn + model.nbVarOut;
-model.dt = 0.01; %Time step duration
-model.rfactor = 1E-2; %Control cost in LQR
+model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+model.dt = 1E-2; %Time step duration
+model.rfactor = 5E-4; %Control cost in LQR
model.tfactor = 1E-2; %Teleoperator cost
+R = eye(model.nbVarOut) * model.rfactor; %Control cost matrix
-%Dynamical System settings
-A = kron([0 1; 0 0], eye(model.nbVarOut));
-B = kron([0; 1], eye(model.nbVarOut));
-%Control cost matrix
-R = eye(model.nbVarOut) * model.rfactor;
+imgVis = 1; %Visualization option
+if imgVis==1
+ global img alpha
+ [img, ~, alpha] = imread('data/drill06.png');
+ % img = imresize(img, .2);
+ % alpha = imresize(alpha, .2);
+end
+
+
+%% Discrete dynamical System settings
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+A1d = zeros(model.nbDeriv);
+for i=0:model.nbDeriv-1
+ A1d = A1d + diag(ones(model.nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
+end
+B1d = zeros(model.nbDeriv,1);
+for i=1:model.nbDeriv
+ B1d(model.nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
+end
+A = kron(A1d, eye(model.nbVarOut)); %Discrete nD
+B = kron(B1d, eye(model.nbVarOut)); %Discrete nD
%% Generate data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-posObj = [ones(1,nbData)*0; ones(1,nbData)*-0.3; mvnrnd(0,eye(model.nbVarOut)*1E-3,nbData)']; %Object position and orientation
-Data0 = [repmat([0.2;-0.2],1,nbData/2)+randn(2,nbData/2)*1E-1, repmat([0.2;0.2],1,nbData/2)+randn(2,nbData/2)*1E-1]; %Path of robot/teleoperator
-Data0 = [Data0; [posObj(end,1:nbData/2)-pi/2+randn(1,nbData/2)*1E-3, linspace(0,1,nbData/2)+randn(1,nbData/2)*1E-1]];
+posObj = [zeros(1,nbData); ones(1,nbData)*-1500; mvnrnd(0,eye(model.nbVarOut)*1E-4,nbData)']; %Object position and orientation
+Data0 = [repmat([1000;-1100],1,nbData/2)+diag([1E3,4E2])*randn(2,nbData/2), repmat([1000;1000],1,nbData/2)+randn(2,nbData/2)*4E2]; %Path of robot/teleoperator (first half close to plane, second half far from plane)
+Data0 = [Data0; [posObj(end,1:nbData/2)-pi/2+randn(1,nbData/2)*1E-5, (rand(1,nbData/2)-0.5)*2*pi]]; %Concatenation of orientation data (first half close to plane, second half far from plane)
%Set task parameters
for t=1:nbData
model.p(1,t).b = posObj(:,t); %param1 (object)
for m=1:model.nbFrames
- %model.p(m,t).A = eye(model.nbVar);
model.p(m,t).A = [cos(posObj(end,t)), -sin(posObj(end,t)), 0; sin(posObj(end,t)) cos(posObj(end,t)) 0; 0 0 1];
model.p(m,t).invA = inv(model.p(m,t).A); %Precomputation of inverse
end
@@ -85,71 +102,71 @@ model = init_tensorGMM_kmeans(Data, model);
model = EM_tensorGMM(Data, model);
-%% Reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Reproduction\n');
-
-%Simulate object and teleoperator trajectories
-rPosObj = [ones(1,nbData)*0; ones(1,nbData)*-0.2; ones(1,nbData).*-0.2]; %Object position and orientation
-
-aTmp = linspace(0,4*pi,nbData);
-rPosTel = [ones(1,nbData).*aTmp*(0.02/4*pi)+sin(aTmp)*0.05+0.1; cos(aTmp)*0.4; ones(1,nbData).*0.2]; %Teleoperator position and orientation
-
-x = rPosTel(model.nbVarIn+1:model.nbVarIn+model.nbVarOut,1);
-dx = zeros(model.nbVarOut,1);
-for t=1:nbData
- %Frame1 (object)
- %pTmp(1).A = eye(model.nbVar);
- pTmp(1).A = [cos(rPosObj(end,t)), -sin(rPosObj(end,t)), 0; sin(rPosObj(end,t)) cos(rPosObj(end,t)) 0; 0 0 1];
- pTmp(1).b = rPosObj(:,t);
-
- %GMR with GMM adapted to the current situation
- mtmp.nbStates = model.nbStates;
- mtmp.Priors = model.Priors;
- for i=1:mtmp.nbStates
- mtmp.Mu(:,i) = pTmp(1).A * squeeze(model.Mu(:,1,i)) + pTmp(1).b;
- mtmp.Sigma(:,:,i) = pTmp(1).A * squeeze(model.Sigma(:,:,1,i)) * pTmp(1).A';
- end
- [MuOut(:,1), SigmaOut(:,:,1)] = GMR(mtmp, rPosTel(1:model.nbVarIn,t), 1:model.nbVarIn, model.nbVarIn+1:model.nbVarIn+model.nbVarOut);
-
- %Second Gaussian as teleoperator
- MuOut(:,2) = rPosTel(model.nbVarIn+1:model.nbVarIn+model.nbVarOut,t);
- SigmaOut(:,:,2) = eye(model.nbVarOut) * model.tfactor;
-
- %Product of Gaussians
- SigmaTmp = zeros(model.nbVarOut);
- MuTmp = zeros(model.nbVarOut,1);
- for m=1:2
- SigmaTmp = SigmaTmp + inv(SigmaOut(:,:,m));
- MuTmp = MuTmp + SigmaOut(:,:,m) \ MuOut(:,m);
- end
- rMu(:,t) = SigmaTmp \ MuTmp;
- rSigma(:,:,t) = inv(SigmaTmp);
-
- %Linear quadratic tracking (infinite horizon)
- Q = zeros(model.nbVarOut*2);
- Q(1:model.nbVarOut,1:model.nbVarOut) = SigmaTmp;
- P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2);
- L = R\B'*P; %Feedback term
- ddx = L * ([rMu(:,t); zeros(model.nbVarOut,1)] - [x; dx]); %Compute acceleration (with only feedback terms)
- dx = dx + ddx * model.dt;
- x = x + dx * model.dt;
- rData(:,t) = [rPosTel(1:model.nbVarIn,t); x];
-end
+% %% Simulate reproduction
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% fprintf('Reproduction\n');
+%
+% %Simulate object and teleoperator trajectories
+% rPosObj = [zeros(1,nbData); ones(1,nbData)*-200; ones(1,nbData).*-0.2]; %Object position and orientation
+%
+% aTmp = linspace(0,4*pi,nbData);
+% rPosTel = [ones(1,nbData).*aTmp*(0.02/4*pi)+sin(aTmp)*0.05+0.1; cos(aTmp)*0.4; ones(1,nbData).*0.2]; %Teleoperator position and orientation
+%
+% rPosTel(1:2,:) = rPosTel(1:2,:) * 1000;
+%
+% x = rPosTel(model.nbVarIn+1:model.nbVarIn+model.nbVarOut, 1);
+% dx = zeros(model.nbVarOut,1);
+% for t=1:nbData
+% %Frame1 (object)
+% %pTmp(1).A = eye(model.nbVar);
+% pTmp(1).A = [cos(rPosObj(end,t)), -sin(rPosObj(end,t)), 0; sin(rPosObj(end,t)) cos(rPosObj(end,t)) 0; 0 0 1];
+% pTmp(1).b = rPosObj(:,t);
+%
+% %GMR with GMM adapted to the current situation
+% mtmp.nbStates = model.nbStates;
+% mtmp.Priors = model.Priors;
+% for i=1:mtmp.nbStates
+% mtmp.Mu(:,i) = pTmp(1).A * squeeze(model.Mu(:,1,i)) + pTmp(1).b;
+% mtmp.Sigma(:,:,i) = pTmp(1).A * squeeze(model.Sigma(:,:,1,i)) * pTmp(1).A';
+% end
+% [MuOut(:,1), SigmaOut(:,:,1)] = GMR(mtmp, rPosTel(1:model.nbVarIn,t), 1:model.nbVarIn, model.nbVarIn+1:model.nbVarIn+model.nbVarOut);
+%
+% %Second Gaussian as teleoperator
+% MuOut(:,2) = rPosTel(model.nbVarIn+1:model.nbVarIn+model.nbVarOut,t);
+% SigmaOut(:,:,2) = eye(model.nbVarOut) * model.tfactor;
+%
+% %Product of Gaussians
+% SigmaTmp = zeros(model.nbVarOut);
+% MuTmp = zeros(model.nbVarOut,1);
+% for m=1:2
+% SigmaTmp = SigmaTmp + inv(SigmaOut(:,:,m));
+% MuTmp = MuTmp + SigmaOut(:,:,m) \ MuOut(:,m);
+% end
+% rMu(:,t) = SigmaTmp \ MuTmp;
+% rSigma(:,:,t) = inv(SigmaTmp);
+%
+% %Linear quadratic tracking (infinite horizon)
+% Q = zeros(model.nbVarOut*2);
+% Q(1:model.nbVarOut,1:model.nbVarOut) = SigmaTmp;
+% P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2);
+% L = R\B'*P; %Feedback term
+% ddx = L * ([rMu(:,t); zeros(model.nbVarOut,1)] - [x; dx]); %Compute acceleration (with only feedback terms)
+% dx = dx + ddx * model.dt;
+% x = x + dx * model.dt;
+% rData(:,t) = [rPosTel(1:model.nbVarIn,t); x];
+% end
% %% Plot
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% fprintf('Plots\n');
-%
-% figure('position',[20,50,1300,500]);
+% figure('position',[20,50,2300,900]);
% clrmap = lines(model.nbStates);
% ttl={'Frame 1 (in)','Frame 1 (out)'};
%
% %DEMOS
% subplot(1,4,1); hold on; box on; title('Demonstrations');
% plotPlane(posObj, [0 0 0]);
-% plotEE(Data0, [.7 0 .7]);
+% plotEE(Data0, [.7 0 .7], 0);
% %legend('object','robot','teleoperator');
% axis equal; %set(gca,'xtick',[],'ytick',[]);
%
@@ -162,22 +179,31 @@ end
% axis equal; %set(gca,'xtick',[0],'ytick',[0]);
%
% %FRAME OUT
-% subplot(1,4,3); hold on; grid on; box on; title(ttl{2});
+% % subplot(1,4,3); hold on; grid on; box on; title(ttl{2});
% for i=1:model.nbStates
+% subplot(1,4,2+i); hold on; grid on; box on; title(ttl{2});
% mtmp.nbStates = 1;
% mtmp.Priors = model.Priors(i);
% mtmp.Mu = model.Mu(3,1,i);
% mtmp.Sigma = model.Sigma(3,3,1,i);
-% plotGMM1D(mtmp, clrmap(i,:), [-pi,0,2*pi,1], .3, 50);
+% plotGMM1D(mtmp, [-pi,pi,0,1], clrmap(i,:), .3, 50);
+% axis([-pi,pi,-.05,1.2]);
+% set(gca,'ytick',[0,1],'xtick',[-pi,-pi/2,0,pi/2,pi],'xticklabel',{'-\pi','-\pi/2','0','\pi/2','\pi'});
% end
-% % for t=1:5:nbData
-% % mtmp.nbStates = 1;
-% % mtmp.Priors = 1;
-% % mtmp.Mu = rMu(:,t);
-% % mtmp.Sigma = rSigma(:,:,t);
-% % plotGMM1D(mtmp, [.2 .2 .2], [-1,0,5,1], .3, 50);
-% % end
%
+% %print('-dpng','graphs/demo_TPGMM_teleoperation_model01.png');
+% pause;
+% close all;
+% return
+
+
+% for t=1:5:nbData
+% mtmp.nbStates = 1;
+% mtmp.Priors = 1;
+% mtmp.Mu = rMu(:,t);
+% mtmp.Sigma = rSigma(:,:,t);
+% plotGMM1D(mtmp, [.2 .2 .2], [-1,0,5,1], .3, 50);
+% end
% %REPRO
% subplot(1,4,4); hold on; grid on; box on; title('Reproduction');
% axis equal;
@@ -189,75 +215,43 @@ end
%
% %print('-dpng','graphs/demo_TPGMM_teleoperation02.png');
% pause;
-% close all;
-
-
-% %% Plot
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% clrmap = lines(model.nbStates);
-% figure('position',[20,50,1300,500]);
-% %FRAME IN
-% subplot(1,2,1); hold on; grid on; box on;
-% %plot(squeeze(Data(1,1,:)), squeeze(Data(2,1,:)), '.','markersize',15,'color',[.7 0 .7]);
-% for i=1:model.nbStates
-% plotGMM(squeeze(model.Mu(1:2,1,i)), squeeze(model.Sigma(1:2,1:2,1,i)), clrmap(i,:), .4);
-% end
-% plot(0, 0, '+','markersize',15,'linewidth',2,'color',[0 0 0]);
-% axis([-0.1,0.4,-0.1 0.8]);
-% set(gca,'xtick',[0],'ytick',[0],'fontsize',18);
-%
-% %FRAME OUT
-% subplot(1,2,2); hold on; grid on; box on;
-% for i=1:model.nbStates
-% mtmp.nbStates = 1;
-% mtmp.Priors = model.Priors(i);
-% mtmp.Mu = model.Mu(3,1,i);
-% mtmp.Sigma = model.Sigma(3,3,1,i) + 1E-3;
-% plotGMM1D(mtmp, clrmap(i,:), [-pi,0,2*pi,1], .3, 200);
-% end
-% axis([-pi,pi,0,1.2]);
-% set(gca,'xtick',[-pi,-pi/2,0,pi/2,pi],'xticklabel',{'-pi','-pi/2','0','pi/2','pi'},'ytick',[],'fontsize',18);
-%
-% %print('-dpng','graphs/demo_TPGMM_teleoperation_model01.png');
-% pause;
-% close all;
+% % close all;
%% Interactive plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fig = figure('position',[10,10,1300,1300],'name','Move the robot with the mouse and wheel mouse'); hold on; box on;
+fig = figure('position',[10,10,700,1300],'name','Move the robot with the mouse and wheel mouse'); hold on; box on;
set(fig,'WindowButtonMotionFcn',{@wbm});
set(fig,'WindowButtonDownFcn',{@wbd});
set(fig,'WindowScrollWheelFcn',{@wsw});
-set(fig,'CloseRequestFcn',@crq);
+set(fig,'CloseRequestFcn',{@crq});
+H = uicontrol('Style','PushButton','String','Exit','Callback','delete(gcbf)');
setappdata(gcf,'mw',0);
-setappdata(gcf,'cr',0);
set(gca,'Xtick',[]); set(gca,'Ytick',[]);
-axis equal; axis([0 .3 -0.3 0.3]);
-clrmap = lines(model.nbStates);
-
-rpo = rPosObj(:,1);
-%xt = zeros(3,1);
-xt = [zeros(2,1); -pi/2];
-%x = zeros(model.nbVarOut,1);
-x = -pi/2;
-dx = zeros(model.nbVarOut,1);
-h = [];
-id = 1;
-while getappdata(gcf,'cr')==0
+axis equal; axis([0 2000 -1500 1500]);
+
+rpo = [0; -1000; -0.2]; %Object position and orientation
+xt = [zeros(model.nbVarIn,1); -pi];
+x = [-pi/2; 0];
+hp = [];
+id = 1; idf = 1;
+while (ishandle(H))
cur_point = get(gca,'Currentpoint');
xt(1:2) = cur_point(1,1:2)';
xt(3) = xt(3) + getappdata(gcf,'mw') * 0.1;
setappdata(gcf,'mw',0);
+
%Frame1 (object)
offtmp = getappdata(gcf,'mb');
if offtmp~=0
- rpo(end) = rpo(end) + offtmp * 0.1;
+ rpo(end) = rpo(end) + offtmp * 0.3;
setappdata(gcf,'mb',0);
end
pTmp(1).A = [cos(rpo(end)), -sin(rpo(end)), 0; sin(rpo(end)) cos(rpo(end)) 0; 0 0 1];
pTmp(1).b = rpo;
+
+
%GMM adapted to the current situation
mtmp = [];
mtmp.nbStates = model.nbStates;
@@ -266,11 +260,38 @@ while getappdata(gcf,'cr')==0
mtmp.Mu(:,i) = pTmp(1).A * squeeze(model.Mu(:,1,i)) + pTmp(1).b;
mtmp.Sigma(:,:,i) = pTmp(1).A * squeeze(model.Sigma(:,:,1,i)) * pTmp(1).A';
end
-
- %GMR
- [MuOut(:,1), SigmaOut(:,:,1)] = GMR(mtmp, xt(1:model.nbVarIn), 1:model.nbVarIn, model.nbVarIn+1:model.nbVarIn+model.nbVarOut);
- %Second Gaussian as teleoperator
+
+ %GMR
+ in = 1:model.nbVarIn;
+ out = model.nbVarIn+1:model.nbVar;
+ MuOut = zeros(model.nbVarOut, 1);
+ MuTmp = zeros(model.nbVarOut, model.nbStates);
+ SigmaOut = zeros(model.nbVarOut, model.nbVarOut, 1);
+ SigmaYX = zeros(model.nbVarOut, model.nbVarOut, model.nbStates);
+ for i=1:model.nbStates
+ SigmaYX(:,:,i) = mtmp.Sigma(out,out,i); % - mtmp.Sigma(out,in,i) / mtmp.Sigma(in,in,i) * mtmp.Sigma(in,out,i);
+ end
+ %Compute activation weight
+ for i=1:model.nbStates
+ h(i) = gaussPDF(xt(in), mtmp.Mu(in,i), mtmp.Sigma(in,in,i));
+ end
+ h = h ./ (sum(h)+realmin);
+ %Compute conditional means
+ for i=1:model.nbStates
+ MuTmp(:,i) = mtmp.Mu(out,i); % + mtmp.Sigma(out,in,i) / mtmp.Sigma(in,in,i) * (xt(in) - mtmp.Mu(in,i));
+ MuOut = MuOut + h(i) * MuTmp(:,i);
+ end
+ %Compute conditional covariances
+ for i=1:model.nbStates
+ SigmaOut = SigmaOut + h(i) * SigmaYX(:,:,i);
+% SigmaOut = SigmaOut + h(i) * (SigmaYX(:,:,i) + MuTmp(:,i) * MuTmp(:,i)');
+ end
+% SigmaOut = SigmaOut - MuOut*MuOut' + eye(model.nbVarOut) * model.params_diagRegFact;
+
+
+
+ %Set second Gaussian as teleoperator
MuOut(:,2) = xt(model.nbVarIn+1:model.nbVarIn+model.nbVarOut);
SigmaOut(:,:,2) = eye(model.nbVarOut) * model.tfactor;
@@ -283,63 +304,88 @@ while getappdata(gcf,'cr')==0
end
rMu = SigmaTmp \ MuTmp;
- %rMu = MuOut(:,2); %Simulate demo phase
- %Linear quadratic tracking (infinite horizon)
- Q = zeros(model.nbVarOut*2);
- Q(1:model.nbVarOut,1:model.nbVarOut) = SigmaTmp;
- P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2);
- L = R\B'*P; %Feedback term
- ddx = L * ([rMu; zeros(model.nbVarOut,1)] - [x; dx]); %Compute acceleration (with only feedback terms)
- dx = dx + ddx * model.dt;
- x = x + dx * model.dt;
+ %Linear quadratic tracking (infinite horizon, discrete version)
+ Q = blkdiag(SigmaTmp, zeros(model.nbVarOut));
+ P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), Q);
+ L = (B' * P * B + R) \ B' * P * A; %Feedback gain (discrete version)
+ u = L * ([rMu; zeros(model.nbVarOut,1)] - x); %Compute acceleration commands (with only feedback terms)
+ x = A * x + B * u;
xr = [xt(1:model.nbVarIn); x];
- xh = [xt(1:model.nbVarIn); rMu];
- xh2 = [xt(1:model.nbVarIn); MuOut(:,1)];
+% xh = [xt(1:model.nbVarIn); rMu];
+% xh2 = [xt(1:model.nbVarIn); MuOut(:,1)];
+
%Plot
- delete(h);
- h = plotPlane(rpo,[0 0 0]);
- %for i=1:model.nbStates
- % h = [h plotGMM(mtmp.Mu(1:model.nbVarIn,i), mtmp.Sigma(1:model.nbVarIn,1:model.nbVarIn,i), clrmap(i,:), .4)];
- %end
- h = [h plotEE(xt,[0 0 0])];
- h = [h plotEE(xr,[.8 0 0])];
- %h = [h plotEE(xh,[0 .7 0])];
- %h = [h plotEE(xh2,[.5 1 .5])];
+ delete(hp); hp=[];
+% for i=1:model.nbStates
+% hp = [hp, plotGMM(mtmp.Mu(1:model.nbVarIn,i), mtmp.Sigma(1:model.nbVarIn,1:model.nbVarIn,i), [1 .7 .7], .3)];
+% end
+ hp = [hp, plotPlane(rpo,[0 0 0])];
+ hp = [hp, plotEE(xt, [.8 .8 1], imgVis)];
+ hp = [hp, plotEE(xr, [0 0 .8], imgVis)];
+% hp = [hp plotEE(xh, [0 .7 0], 0)];
+% hp = [hp plotEE(xh2, [.8 0 0], 0)];
drawnow;
- %print('-dpng',['graphs/anim/teleop_demo' num2str(id,'%.3d') '.png']);
- %print('-dpng',['graphs/anim/teleop_repro' num2str(id,'%.3d') '.png']);
+% print('-dpng',['graphs/anim/teleop_drill' num2str(id,'%.3d') '.png']);
id = id+1;
+ if mod(id,10)==0
+ print('-dpng',['graphs/anim/teleop_drill' num2str(idf,'%.3d') '.png']);
+ idf = idf+1;
+ end
end
-delete(gcf);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function h = plotEE(x,col)
-h = plot(x(1,:), x(2,:), '.','markersize',20,'color',col);
-for t=1:size(x,2)
- msh = [x(1:2,t), x(1:2,t)+[cos(x(3,t)); sin(x(3,t))]*0.03];
- h = [h plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',col)];
+function h = plotEE(x, col, imgVis)
+global img alpha
+if imgVis==1
+ h = [];
+ if col(1)==0
+ img2 = imrotate(img, rad2deg(-x(3,1)));
+ alpha2 = imrotate(alpha, rad2deg(-x(3,1)));
+ h = [h, image(x(1,1)-size(img2,1)/2, x(2,1)-size(img2,2)/2, img2, 'AlphaData', alpha2)];
+ else
+ img2 = imrotate(img, rad2deg(-x(3,1)));
+ alpha2 = imrotate(alpha, rad2deg(-x(3,1)));
+ h = [h, image(x(1,1)-size(img2,1)/2, x(2,1)-size(img2,2)/2, img2, 'AlphaData', alpha2.*.3)];
+ end
+else
+ h = plot(x(1,:), x(2,:), '.','markersize',20,'color',col);
+ for t=1:size(x,2)
+ msh = [x(1:2,t), x(1:2,t) + [cos(x(3,t)); sin(x(3,t))] .* 150];
+ h = [h, plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',col)];
+ end
end
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPlane(x,col)
-h = []; %plot(x(1,:), x(2,:), '.','markersize',20,'color',col);
+h = [];
+%plot(x(1,:), x(2,:), '.','markersize',20,'color',col);
for t=1:size(x,2)
- msh = [x(1:2,t), x(1:2,t)+[cos(x(3,t)); sin(x(3,t))]*0.5];
+ msh = [x(1:2,t), x(1:2,t) + [cos(x(3,t)); sin(x(3,t))] .* 2100];
h = [h plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',col)];
end
+if size(x,2)==1
+ msh = [msh, msh(:,2)-[0;100], msh(:,1)-[0;100]];
+ h = [h patch(msh(1,:), msh(2,:), [.7 .7 .7],'linestyle','none')];
+ h = [h plot2DArrow(mean(msh,2), [cos(x(3,1)); sin(x(3,1))] .* 250, [.8 0 0], 4, 5E1)];
+ h = [h plot2DArrow(mean(msh,2), [-sin(x(3,1)); cos(x(3,1))] .* 250, [.8 0 0], 4, 5E1)];
+end
+
%% Mouse move
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wbm(h,evd)
+
%% Mouse scroll wheel
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wsw(h,evd)
setappdata(gcf,'mw',evd.VerticalScrollCount);
+
%% Mouse button down
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function wbd(h,evd) % executes when the mouse button is pressed
@@ -349,9 +395,4 @@ if strcmp(muoseside,'normal')==1
end
if strcmp(muoseside,'alt')==1
setappdata(gcf,'mb',1);
-end
-
-%% Close request function
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function crq(h,evd)
-setappdata(gcf,'cr',1);
\ No newline at end of file
+end
\ No newline at end of file
diff --git a/demos/demo_TPGMR01.m b/demos/demo_TPGMR01.m
index 509a26b27261dc13cf19567a388bc9bde914890f..868d098bd10646f01f750c7243a530074cebfd83 100644
--- a/demos/demo_TPGMR01.m
+++ b/demos/demo_TPGMR01.m
@@ -91,7 +91,7 @@ out = 2:model.nbVar;
MuGMR = zeros(length(out), nbData, model.nbFrames);
SigmaGMR = zeros(length(out), length(out), nbData, model.nbFrames);
-%Gaussian mixture regression
+%REPROS
for m=1:model.nbFrames
%Compute activation weights
for i=1:model.nbStates
@@ -114,6 +114,7 @@ for m=1:model.nbFrames
end
end
+%REPROS + NEW REPROS
for n=1:nbSamples+nbRepros
MuTmp = zeros(length(out), nbData, model.nbFrames);
SigmaTmp = zeros(length(out), length(out), nbData, model.nbFrames);
@@ -218,38 +219,6 @@ end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%axis equal;
-
-% %Reproductions in the same situations
-% figure('position',[20,50,500,500]); hold on; axis off;
-% for n=1:nbSamples
-% %Plot frames
-% for m=1:model.nbFrames
-% plot([r(n).p(m).b(2) r(n).p(m).b(2)+r(n).p(m).A(2,3)], [r(n).p(m).b(3) r(n).p(m).b(3)+r(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-% plot(r(n).p(m).b(2), r(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-% end
-% %Plot trajectories
-% plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:));
-% plot(r(n).Data(1,:), r(n).Data(2,:),'-','linewidth',1.5,'color',clrmap(n,:));
-% end
-% axis square; axis(limAxes);
-% print('-dpng','graphs/TP_GMR01.png');
-%
-% %Reproductions in new situations
-% figure('position',[20,50,500,500]); hold on; axis off;
-% for n=1:nbRepros
-% %Plot frames
-% for m=1:model.nbFrames
-% plot([r(nbSamples+n).p(m).b(2) r(nbSamples+n).p(m).b(2)+r(nbSamples+n).p(m).A(2,3)], [r(nbSamples+n).p(m).b(3) r(nbSamples+n).p(m).b(3)+r(nbSamples+n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-% plot(r(nbSamples+n).p(m).b(2), r(nbSamples+n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-% end
-% %Plot trajectories
-% plot(r(nbSamples+n).Data(1,1), r(nbSamples+n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]);
-% plot(r(nbSamples+n).Data(1,:), r(nbSamples+n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
-% end
-% axis square; axis(limAxes);
-% print('-dpng','graphs/TP_GMR02.png');
-
-
% print('-dpng','graphs/demo_TPGMR01.png');
pause;
close all;
diff --git a/demos/demo_TPproMP01.m b/demos/demo_TPproMP01.m
index 8a8d2394f1c46b5cb9dfb3b416501b394376527a..0188f7d73e3d32dc57164681bbe76ec2b55516eb 100644
--- a/demos/demo_TPproMP01.m
+++ b/demos/demo_TPproMP01.m
@@ -209,36 +209,6 @@ for m=1:model.nbFrames
axis equal; axis([-4.5 4.5 -1 8]); set(gca,'xtick',[0],'ytick',[0]);
end
-% %Reproductions in same situations
-% figure('position',[20,50,500,500]); hold on; axis off;
-% for n=1:nbSamples
-% %Plot frames
-% for m=1:model.nbFrames
-% plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-% plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-% end
-% %Plot trajectories
-% plot(s(n).MuTraj(2), s(n).MuTraj(3), '.','markersize',12,'color',clrmap(n,:));
-% plot(s(n).MuTraj(2:model.nbVar:end), s(n).MuTraj(3:model.nbVar:end), '-','lineWidth',1.5,'color',clrmap(n,:));
-% end
-% axis square; axis(limAxes);
-% print('-dpng','graphs/TP_ProMP01.png');
-%
-% %Reproductions in new situations
-% figure('position',[20,50,500,500]); hold on; axis off;
-% for n=1:nbRepros
-% %Plot frames
-% for m=1:model.nbFrames
-% plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-% plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-% end
-% %Plot trajectories
-% plot(rnew(n).MuTraj(2), rnew(n).MuTraj(3), '.','markersize',12,'color',clrmap(n,:));
-% plot(rnew(n).MuTraj(2:model.nbVar:end), rnew(n).MuTraj(3:model.nbVar:end), '-','lineWidth',1.5,'color',[.2 .2 .2]);
-% end
-% axis square; axis(limAxes);
-% print('-dpng','graphs/TP_ProMP02.png');
-
% print('-dpng','graphs/demo_TPproMP01.png');
pause;
close all;
diff --git a/demos/demo_TPtrajDistrib01.m b/demos/demo_TPtrajDistrib01.m
index 75e37e28e617e15105c50d308b32cb9afe082116..4a166a942194e2977f81f3840e5cc9047c0a8bc1 100644
--- a/demos/demo_TPtrajDistrib01.m
+++ b/demos/demo_TPtrajDistrib01.m
@@ -177,36 +177,6 @@ for n=1:nbRepros
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-% %Reproductions in same situations
-% figure('position',[20,50,500,500]); hold on; axis off;
-% for n=1:nbSamples
-% %Plot frames
-% for m=1:model.nbFrames
-% plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-% plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-% end
-% %Plot trajectories
-% plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:));
-% plot(r(n).Data(1,:), r(n).Data(2,:),'-','linewidth',1.5,'color',clrmap(n,:));
-% end
-% axis square; axis(limAxes);
-% print('-dpng','graphs/TP_trajDistrib01.png');
-%
-% %Reproductions in new situations
-% figure('position',[20,50,500,500]); hold on; axis off;
-% for n=1:nbRepros
-% %Plot frames
-% for m=1:model.nbFrames
-% plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-% plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-% end
-% %Plot trajectories
-% plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]);
-% plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
-% end
-% axis square; axis(limAxes);
-% print('-dpng','graphs/TP_trajDistrib02.png');
-
%print('-dpng','graphs/demo_TPtrajDistrib01.png');
pause;
close all;