Commit e571f71e authored by Sylvain CALINON's avatar Sylvain CALINON

All examples moved to the demos/ folder

parent 1fabb8a6
......@@ -2,14 +2,16 @@
PbDlib is a set of tools combining statistical learning, dynamical systems and optimal control approaches for programming-by-demonstration applications (see http://www.idiap.ch/software/pbdlib/ for details).
The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, Idiap Research Institute. A C++ version of the library (with currently fewer functionalities) is available at https://gitlab.idiap.ch/rli/pbdlib
The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, Idiap Research Institute.
Other versions of the library (in C++ or Python, with currently fewer functionalities) are available at http://www.idiap.ch/software/pbdlib/
### References
Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used parts of these codes.
<br><br>
[1] Tutorial (GMM, TP-GMM, MFA, MPPCA, GMR, LWR, GPR, MPC, LQR, trajGMM):
[1] Tutorial (GMM, TP-GMM, MFA, MPPCA, GMR, LWR, GPR, MPC, LQR, trajGMM): [Link to publication](http://calinon.ch/papers/Calinon-JIST2015.pdf)
```
@article{Calinon16JIST,
author="Calinon, S.",
......@@ -24,7 +26,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[2] HMM, HSMM:
[2] HMM, HSMM: [Link to publication](http://calinon.ch/papers/Rozo-Frontiers2016.pdf)
```
@article{Rozo16Frontiers,
author="Rozo, L. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
......@@ -39,7 +41,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[3] Riemannian manifolds (S2,S3):
[3] Riemannian manifolds (S2,S3): [Link to publication](http://calinon.ch/papers/Zeestraten-RAL2017.pdf)
```
@article{Zeestraten17RAL,
author="Zeestraten, M. J. A. and Havoutis, I. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
......@@ -54,7 +56,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[4] Riemannian manifolds (S+):
[4] Riemannian manifolds (S+): [Link to publication](http://calinon.ch/papers/Jaquier-IROS2017.pdf)
```
@inproceedings{Jaquier17IROS,
author="Jaquier, N. and Calinon, S.",
......@@ -67,7 +69,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[5] Semi-tied GMM:
[5] Semi-tied GMM: [Link to publication](http://calinon.ch/papers/Tanwani-RAL2016.pdf)
```
@article{Tanwani16RAL,
author="Tanwani, A. K. and Calinon, S.",
......@@ -82,7 +84,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[6] DP-means:
[6] DP-means: [Link to publication](http://calinon.ch/papers/Bruno-AURO2017.pdf)
```
@article{Bruno17AURO,
author="Bruno, D. and Calinon, S. and Caldwell, D. G.",
......@@ -209,17 +211,13 @@ All the examples are located in the main folder, and the functions are located i
### Usage
Examples starting with `demo_` can be run from Matlab/GNU Octave.
### Compatibility
The codes are compatible with both Matlab and GNU Octave.
Examples starting with `demo_` can be run as examples. The codes are compatible with both Matlab and GNU Octave.
### License
Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
The Matlab/GNU Octave version of PbDlib is currently maintained by Sylvain Calinon, Idiap Research Institute.
Maintained by Sylvain Calinon, http://calinon.ch/
Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
PbDlib is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License version 3 as
......
function demo_GMR_polyFit01
% Polynomial fitting with multivariate GMR
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
% journal="Intelligent Service Robotics",
% publisher="Springer Berlin Heidelberg",
% doi="10.1007/s11370-015-0187-9",
% year="2016",
% volume="9",
% number="1",
% pages="1--29"
% }
%
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of states in the GMM
model.nbVarIn = 3; %Dimension of input vector
model.nbVarOut = 1; %Dimension of output vector
model.nbVar = model.nbVarIn + model.nbVarOut; %Number of variables (input+output)
nbData = 200; %Length of a trajectory
%% Load data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/DataLS01.mat');
X = [];
for i=1:model.nbVarIn
X = [X, x.^i]; %-> X=[x, x.^2, x.^3]
end
Data = [X'; Y'] + randn(model.nbVar,size(X,1))*1E-5;
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_kmeans(Data, model);
model = EM_GMM(Data, model);
model.Sigma0 = model.Sigma;
%Regularization term on the inputs
model.Sigma(1:model.nbVarIn,1:model.nbVarIn,:) = model.Sigma(1:model.nbVarIn,1:model.nbVarIn,:) + repmat(eye(model.nbVarIn)*1E3,[1,1,model.nbStates]);
%Regression
xr = linspace(min(x),max(x),nbData);
DataIn = [];
for i=1:model.nbVarIn
DataIn = [DataIn; xr.^i]; %-> X=[x, x.^2, x.^3]
end
DataOut = GMR(model, DataIn, 1:model.nbVarIn, model.nbVarIn+1:model.nbVar);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,500]); hold on; %axis off;
plotGMM(model.Mu([1,end],:), model.Sigma0([1,end],[1,end],:), [.8 .8 .8]);
plot(DataIn(1,:),DataOut(1,:),'-','linewidth',2,'color',[1 .6 .6]);
plot(DataIn(1,:),DataOut(1,:),'.','markersize',6,'color',[.8 0 0]);
plot(Data(1,:),Data(end,:),'.','markersize',16,'color',[.2 .2 .2]);
xlabel('x_1'); ylabel('y_1');
axis([min(DataIn(1,:))-0.1, max(DataIn(1,:))+0.1, min(DataOut(1,:))-0.1, max(DataOut(1,:))+0.1]);
%print('-dpng','graphs/demo_GMRpolyFit01.png');
%pause;
%close all;
function demo_TPtrajDistrib01
% Task-parameterized model with trajectory distribution and eigendecomposition
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
% journal="Intelligent Service Robotics",
% publisher="Springer Berlin Heidelberg",
% doi="10.1007/s11370-015-0187-9",
% year="2016",
% volume="9",
% number="1",
% pages="1--29"
% }
%
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbFrames = 2; %Number of candidate frames of reference
model.nbVar = 2; %Dimension of data (here: x1,x2)
nbRepros = 5; %Number of reproductions with new situations randomly generated
nbData = 200; %Number of datapoints in a trajectory
nbEigs = 3; %Number of principal eigencomponents to keep
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...');
% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=2 the dimension of a
% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
% multiplied by the number of demonstrations (nbSamples=5).
load('data/Data01.mat');
%% Compute trajectory distributions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for m=1:model.nbFrames
DataTmp = reshape(Data(:,m,:),model.nbVar*nbData,nbSamples);
model.Mu(:,m) = mean(DataTmp,2);
model.Sigma(:,:,m) = cov(DataTmp');
%Keep only a few principal eigencomponents
[V,D] = eig(model.Sigma(:,:,m));
[d,id] = sort(diag(D),'descend');
model.D(:,:,m) = diag(d(1:nbEigs));
model.V(:,:,m) = V(:,id(1:nbEigs));
model.Sigma(:,:,m) = model.V(:,:,m) * model.D(:,:,m) * model.V(:,:,m)' + eye(model.nbVar*nbData)*1E-2;
end
%% Reproduction for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions...');
for n=1:nbSamples
%Product of linearly transformed trajectory distribution
SigmaTmp = zeros(model.nbVar*nbData);
MuTmp = zeros(model.nbVar*nbData,1);
for m=1:model.nbFrames
A = kron(eye(nbData), s(n).p(m).A);
b = kron(ones(nbData,1), s(n).p(m).b);
MuP = A * model.Mu(:,m) + b;
SigmaP = A * model.Sigma(:,:,m) * A';
SigmaTmp = SigmaTmp + inv(SigmaP);
MuTmp = MuTmp + SigmaP\MuP;
end
expSigma = inv(SigmaTmp);
r(n).Data = reshape(expSigma*MuTmp, model.nbVar, nbData);
for t=1:nbData
id = (t-1)*model.nbVar+1:t*model.nbVar;
r(n).expSigma(:,:,t) = expSigma(id,id);
end
end
%% Reproduction for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('New reproductions...');
for n=1:nbRepros
for m=1:model.nbFrames
%Random generation of new task parameters
id=ceil(rand(2,1)*nbSamples);
w=rand(2); w=w/sum(w);
rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
end
%Product of linearly transformed trajectory distribution
SigmaTmp = zeros(model.nbVar*nbData);
MuTmp = zeros(model.nbVar*nbData,1);
for m=1:model.nbFrames
A = kron(eye(nbData), rnew(n).p(m).A);
b = kron(ones(nbData,1), rnew(n).p(m).b);
MuP = A * model.Mu(:,m) + b;
SigmaP = A * model.Sigma(:,:,m) * A';
SigmaTmp = SigmaTmp + inv(SigmaP);
MuTmp = MuTmp + SigmaP\MuP;
end
expSigma = inv(SigmaTmp);
rnew(n).Data = reshape(expSigma*MuTmp, model.nbVar, nbData);
for t=1:nbData
id = (t-1)*model.nbVar+1:t*model.nbVar;
rnew(n).expSigma(:,:,t) = expSigma(id,id);
end
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
xx = round(linspace(1,64,nbSamples));
clrmap = colormap('jet');
clrmap = min(clrmap(xx,:),.95);
limAxes = [-1.2 0.8 -1.1 0.9];
colPegs = [[.9,.5,.9];[.5,.9,.5]];
%DEMOS
subplot(1,3,1); hold on; box on; title('Demonstrations');
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(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%REPROS
subplot(1,3,2); hold on; box on; title('Reproductions');
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
end
for n=1:nbSamples
%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(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%NEW REPROS
subplot(1,3,3); hold on; box on; title('New reproductions');
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
end
for n=1:nbRepros
%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(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%print('-dpng','graphs/demo_TPtrajDistrib01.png');
%pause;
%close all;