Skip to content

GitLab

Commit 8d6dbeee authored by Sylvain CALINON's avatar Sylvain CALINON
Browse files

PbDlib v1.0

parent bd713a21
Expand all Hide whitespace changes
Inline Side-by-side
README.md
View file @ 8d6dbeee
This diff is collapsed.
demos/benchmark_DS_GP_GMM01.m → demos/benchmarks/benchmark_DS_GP_GMM01.m
View file @ 8d6dbeee
......@@ -2,11 +2,7 @@ function benchmark_DS_GP_GMM01
% Benchmark of task-parameterized model based on Gaussian process regression,
% with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction.
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -36,8 +32,7 @@ function benchmark_DS_GP_GMM01
% 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/');
addpath('./../m_fcts/');
%% Parameters
......@@ -137,7 +132,7 @@ for n=1:nbSamples
end
% %Retrieval of attractor path through GMR
% currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
% currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]);
%
% %Motion retrieval with spring-damper system
% x = s(n).p(1).b(2:model.nbVar);
......@@ -181,7 +176,7 @@ for n=1:nbRepros
end
%Retrieval of attractor path through GMR
[rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
[rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]);
%Motion retrieval with spring-damper system
x = rnew(n).p(1).b(2:model.nbVar);
......@@ -230,27 +225,23 @@ for n=1:nbRepros
h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes);
%print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
pause
pause;
end
pause;
close all;
end
%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
\ No newline at end of file
demos/benchmark_DS_GP_raw01.m → demos/benchmarks/benchmark_DS_GP_raw01.m
View file @ 8d6dbeee
......@@ -2,11 +2,7 @@ function benchmark_DS_GP_raw01
% Benchmark of task-parameterized model based on Gaussian process regression,
% with raw trajectory, and spring-damper system used for reproduction.
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -36,7 +32,7 @@ function benchmark_DS_GP_raw01
% 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/');
addpath('./../m_fcts/');
%% Parameters
......@@ -180,22 +176,18 @@ end
pause;
close all;
end
%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
\ No newline at end of file
demos/benchmark_DS_PGMM01.m → demos/benchmarks/benchmark_DS_PGMM01.m
View file @ 8d6dbeee
function benchmark_DS_PGMM01
% Benchmark of task-parameterized model based on parametric Gaussianvmixture model,
% Benchmark of task-parameterized model based on parametric Gaussian mixture model,
% and DS-GMR used for reproduction.
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -36,7 +32,7 @@ function benchmark_DS_PGMM01
% 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/');
addpath('./../m_fcts/');
%% Parameters
......@@ -86,10 +82,10 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of PGMM with EM:');
for n=1:nbSamples
% %Task parameters rearranged as a vector (position and orientation), see Eq. (48)
% %Task parameters rearranged as a vector (position and orientation)
% s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1];
%Task parameters rearranged as a vector (position only), see Eq. (48)
%Task parameters rearranged as a vector (position only)
s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1];
end
......@@ -121,12 +117,12 @@ nbVarOut = model.nbVar-1;
for n=1:nbSamples
%Computation of the resulting Gaussians (for display purpose)
for i=1:model.nbStates
model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (48)
model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable
end
r(n).Mu = model.Mu;
% %Retrieval of attractor path through GMR
% currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
% currTar = GMR(model, DataIn, 1, [2:model.nbVar]);
% %Motion retrieval with spring-damper system
% x = s(n).p(1).b(2:model.nbVar);
% dx = zeros(nbVarOut,1);
......@@ -147,25 +143,25 @@ load('data/taskParams.mat'); %Load new task parameters (new situation)
for n=1:nbRepros
rnew(n).p = taskParams(n).p;
% %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
% %Task parameters re-arranged as a vector (position and orientation)
% rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1];
%Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
%Task parameters re-arranged as a vector (position only)
rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1];
%Computation of the resulting Gaussians (for display purpose)
for i=1:model.nbStates
model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf
model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable
end
rnew(n).Mu = model.Mu;
%Retrieval of attractor path through GMR
[rnew(n).currTar, rnew(n).currSigma] = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
[rnew(n).currTar, rnew(n).currSigma] = GMR(model, DataIn, 1, [2:model.nbVar]);
%Motion retrieval with spring-damper system
x = rnew(n).p(1).b(2:model.nbVar);
dx = zeros(nbVarOut,1);
for t=1:nbD
%Compute acceleration, velocity and position
ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
ddx = -L * [x-rnew(n).currTar(:,t); dx];
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
rnew(n).Data(:,t) = x;
......@@ -212,22 +208,18 @@ end
pause;
close all;
end
%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
\ No newline at end of file
demos/benchmark_DS_TP_GMM01.m → demos/benchmarks/benchmark_DS_TP_GMM01.m
View file @ 8d6dbeee
......@@ -2,11 +2,7 @@ function benchmark_DS_TP_GMM01
% Benchmark of task-parameterized Gaussian mixture model (TP-GMM),
% with DS-GMR used for reproduction.
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -36,7 +32,8 @@ function benchmark_DS_TP_GMM01
% 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/');
addpath('./../m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
......@@ -151,22 +148,18 @@ end
pause;
close all;
end
%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
\ No newline at end of file
demos/benchmark_DS_TP_GP01.m → demos/benchmarks/benchmark_DS_TP_GP01.m
View file @ 8d6dbeee
function benchmark_DS_TP_GP01
% Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method).
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -35,7 +31,7 @@ function benchmark_DS_TP_GP01
% 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/');
addpath('./../m_fcts/');
%% Parameters
......@@ -101,14 +97,14 @@ end
%Reproduction with spring-damper system
% for n=1:nbSamples
% currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
% currTar = productTPGMM0(model, s(n).p);
%
% %Motion retrieval with spring-damper system
% x = s(n).p(1).b;
% dx = zeros(model.nbVar,1);
% for t=1:s(n).nbData
% %Compute acceleration, velocity and position
% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
% ddx = -L * [x-currTar(:,t); dx];
% dx = dx + ddx * model.dt;
% x = x + dx * model.dt;
% r(n).Data(:,t) = x;
......@@ -125,14 +121,14 @@ for n=1:nbRepros
rnew(n).p(m).b = taskParams(n).p(m).b(2:end);
rnew(n).p(m).A = taskParams(n).p(m).A(2:end,2:end);
end
[rnew(n).currTar, rnew(n).currSigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
[rnew(n).currTar, rnew(n).currSigma] = productTPGMM0(model, rnew(n).p);
%Motion retrieval with spring-damper system
x = rnew(n).p(1).b;
dx = zeros(model.nbVar,1);
for t=1:nbD
%Compute acceleration, velocity and position
ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
ddx = -L * [x-rnew(n).currTar(:,t); dx];
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
rnew(n).Data(:,t) = x;
......@@ -170,27 +166,23 @@ for n=1:nbRepros
h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes);
%print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
pause
pause;
end
pause;
close all;
end
%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
\ No newline at end of file
demos/benchmark_DS_TP_LWR01.m → demos/benchmarks/benchmark_DS_TP_LWR01.m
View file @ 8d6dbeee
function benchmark_DS_TP_LWR01
% Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method).
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -35,7 +31,7 @@ function benchmark_DS_TP_LWR01
% 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/');
addpath('./../m_fcts/');
%% Parameters
......@@ -112,14 +108,14 @@ end
%Reproduction with spring-damper system
% for n=1:nbSamples
% currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
% currTar = productTPGMM0(model, s(n).p);
%
% %Motion retrieval with spring-damper system
% x = s(n).p(1).b;
% dx = zeros(model.nbVar,1);
% for t=1:s(n).nbData
% %Compute acceleration, velocity and position
% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
% ddx = -L * [x-currTar(:,t); dx];
% dx = dx + ddx * model.dt;
% x = x + dx * model.dt;
% r(n).Data(:,t) = x;
......@@ -136,14 +132,14 @@ for n=1:nbRepros
rnew(n).p(m).b = taskParams(n).p(m).b(2:end);
rnew(n).p(m).A = taskParams(n).p(m).A(2:end,2:end);
end
[rnew(n).currTar, rnew(n).currSigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
[rnew(n).currTar, rnew(n).currSigma] = productTPGMM0(model, rnew(n).p);
%Motion retrieval with spring-damper system
x = rnew(n).p(1).b;
dx = zeros(model.nbVar,1);
for t=1:nbD
%Compute acceleration, velocity and position
ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
ddx = -L * [x-rnew(n).currTar(:,t); dx];
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
rnew(n).Data(:,t) = x;
......@@ -186,22 +182,18 @@ end
pause;
close all;
end
%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4;
end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p)
dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end
end
\ No newline at end of file
demos/benchmark_DS_TP_MFA01.m → demos/benchmarks/benchmark_DS_TP_MFA01.m
View file @ 8d6dbeee
......@@ -2,11 +2,7 @@ function benchmark_DS_TP_MFA01
% Benchmark of task-parameterized mixture of factor analyzers (TP-MFA),
% with DS-GMR used for reproduction.
%
% 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.
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
......@@ -36,7 +32,7 @@ function benchmark_DS_TP_MFA01
% 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/');
addpath('./../m_fcts/');
%% Parameters
......@@ -49,6 +45,7 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated
nbStochasticRepros = 30; %Number of reproductions with stochastic sampling
%% Load 3rd order tensor data
......@@ -73,7 +70,7 @@ D(end,end) = 0;
%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(nbVarOut));
%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
%Create 3rd order tensor data with XHAT instead of X
Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:);
......@@ -114,6 +111,21 @@ for n=1:nbRepros
end
%% Reproduction with stochastic sampling
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Stochastic sampling with DS-GMR...');
for n=1:nbStochasticRepros
%Retrieval of attractor path through task-parameterized GMR
mtmp = model;
for i=1:mtmp.nbStates
[V,D] = eig(mtmp.Sigma(:,:,i));
mtmp.Mu(:,i) = mtmp.Mu(:,i) + V*D^.5 * 0.8 * randn(model.nbVar,1);
end
asto(n) = estimateAttractorPath(DataIn, mtmp, taskParams(3));
rsto(n) = reproduction_DS(DataIn, mtmp, asto(n), taskParams(3).p(1).b(2:3));
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('PaperPosition',[0 0 4 3],'position',[20,50,600,450]);
......@@ -135,40 +147,49 @@ end
axis equal; axis(limAxes);
%print('-dpng','-r600','graphs/benchmark_DS_TP_MFA01.png');
%Plot reproductions in new situations
% %Plot reproductions in new situations
% disp('[Press enter to see next reproduction attempt]');
% h=[];
% for n=1:nbRepros
% delete(h);
% h = plotPegs(rnew(n).p);
% h = [h plotGMM(rnew(n).currTar, anew(n).currSigma, [0 .8 0], .1)];
% %h = [h plotGMM(rnew(n).Mu(2:3,:), rnew(n).Sigma(2:3,2:3,:), myclr(3,:), .6)];
% h = [h patch([rnew(n).Data(2,:) rnew(n).Data(2,fliplr(1:nbD))], [rnew(n).Data(3,:) rnew(n).Data(3,fliplr(1:nbD))],...
% [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
% h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])];
% axis equal; axis(limAxes);
% %print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA' num2str(n+1,'%.2d') '.png']);
% %pause;
% end
%Plot stochastic sampling in new situations
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
h = plotPegs(rnew(n).p);
h = [h plotGMM(rnew(n).currTar, anew(n).currSigma, [0 .8 0], .2)];
h = [h plotGMM(rnew(n).Mu(2:3,:), rnew(n).Sigma(2:3,2:3,:), myclr(3,:), .6)];
h = [h patch([rnew(n).Data(2,:) rnew(n).Data(2,fliplr(1:nbD))], [rnew(n).Data(3,:) rnew(n).Data(3,fliplr(1:nbD))],...
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes);
%print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA' num2str(n+1,'%.2d') '.png']);
pause;
plotPegs(rsto(1).p);
for n=1:nbStochasticRepros
patch([rsto(n).Data(2,:) rsto(n).Data(2,fliplr(1:nbD))], [rsto(n).Data(3,:) rsto(n).Data(3,fliplr(1:nbD))],...
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.2);
%plot(rsto(n).Data(2,1), rsto(n).Data(3,1),'.','markersize',12,'color',[0 0 0]);
end
axis equal; axis(limAxes);
%print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA_stochastic01.png']);
%pause;
pause;
close all;