Commit 65399a15 authored by Sylvain CALINON's avatar Sylvain CALINON

Ref updated

parent 38cbda64
......@@ -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)
......
......@@ -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
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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;
......
......@@ -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/
......
......@@ -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/
......
......@@ -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/
......
......@@ -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,