benchmark_DS_PGMM01.m 8.39 KB
Newer Older
1
function benchmark_DS_PGMM01
Sylvain CALINON's avatar
Sylvain CALINON committed
2
% Benchmark of task-parameterized model based on parametric Gaussian mixture model, 
3
% and DS-GMR used for reproduction.
Sylvain Calinon's avatar
Sylvain Calinon committed
4
%
Sylvain CALINON's avatar
Sylvain CALINON committed
5
% If this code is useful for your research, please cite the related publication:
6
% @article{Calinon16JIST,
Sylvain Calinon's avatar
Sylvain Calinon committed
7
%   author="Calinon, S.",
8 9
%   title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
%   journal="Intelligent Service Robotics",
10 11 12 13 14 15
%		publisher="Springer Berlin Heidelberg",
%		doi="10.1007/s11370-015-0187-9",
%		year="2016",
%		volume="9",
%		number="1",
%		pages="1--29"
Sylvain Calinon's avatar
Sylvain Calinon committed
16
% }
17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33
% 
% 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/>.
34

Sylvain CALINON's avatar
Sylvain CALINON committed
35
addpath('./../m_fcts/');
36

Sylvain Calinon's avatar
Sylvain Calinon committed
37

38 39 40 41 42 43 44 45 46
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM
model.nbFrames = 2; %Number of candidate frames of reference
model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
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
Sylvain Calinon's avatar
Sylvain Calinon committed
47 48
nbVarOut = model.nbVar-1; %(here, x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
49 50 51 52 53 54 55 56 57


%% 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=3 the dimension of a
Sylvain Calinon's avatar
Sylvain Calinon committed
58 59
% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
% multiplied by the number of demonstrations (M=5).
60 61 62 63 64 65 66 67 68 69 70 71
load('data/DataLQR01.mat');


%% Transformation of 'Data' to learn the path of the spring-damper system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbD = s(1).nbData;
%Create transformation matrix to compute [X; DX; DDX]
D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
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));
Sylvain Calinon's avatar
Sylvain Calinon committed
72
%Compute derivatives
73 74 75 76 77 78 79 80 81 82 83 84
Data = [];
for n=1:nbSamples
	DataTmp = s(n).Data0(2:end,:);
	s(n).Data = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
	Data = [Data s(n).Data];
end


%% PGMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of PGMM with EM:');
for n=1:nbSamples	
Sylvain CALINON's avatar
Sylvain CALINON committed
85
% 	%Task parameters rearranged as a vector (position and orientation)
86
% 	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];
Sylvain Calinon's avatar
Sylvain Calinon committed
87

Sylvain CALINON's avatar
Sylvain CALINON committed
88
	%Task parameters rearranged as a vector (position only)
89 90 91 92 93 94 95
	s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1];
end

% %Initialization of model parameters
% %model = init_GMM_kmeans(Data, model);
% model = init_GMM_timeBased(Data, model);
% model = EM_GMM(Data, model);
Sylvain Calinon's avatar
Sylvain Calinon committed
96

97 98 99 100
for i=1:model.nbStates
% 	%Initialization of parameters based on standard GMM
% 	model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1));
% 	model.ZMu(:,end,i) = model.Mu(:,i);
Sylvain Calinon's avatar
Sylvain Calinon committed
101

102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119
		%Random initialization of parameters
		model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1));
		model.Sigma(:,:,i) = eye(model.nbVar);
end
model.Priors = ones(model.nbStates) / model.nbStates; %Useful when using random initialization of parameters

%PGMM parameters estimation
model = EM_stdPGMM(s, model);


%% Reproduction with PGMM and DS-GMR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with DS-GMR...');
DataIn = [1:s(1).nbData] * model.dt;
nbVarOut = model.nbVar-1;
for n=1:nbSamples
	%Computation of the resulting Gaussians (for display purpose)
	for i=1:model.nbStates
Sylvain CALINON's avatar
Sylvain CALINON committed
120
		model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable
121 122
	end
	r(n).Mu = model.Mu;
Sylvain Calinon's avatar
Sylvain Calinon committed
123
	
124
% 	%Retrieval of attractor path through GMR
Sylvain CALINON's avatar
Sylvain CALINON committed
125
% 	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); 
126 127 128 129 130
% 	%Motion retrieval with spring-damper system
% 	x = s(n).p(1).b(2:model.nbVar);
% 	dx = zeros(nbVarOut,1);
% 	for t=1:s(n).nbData
% 		%Compute acceleration, velocity and position
Sylvain Calinon's avatar
Sylvain Calinon committed
131
% 		ddx =  -L * [x-currTar(:,t); dx]; 
132 133 134 135 136 137 138 139 140 141 142 143 144 145
% 		dx = dx + ddx * model.dt;
% 		x = x + dx * model.dt;
% 		r(n).Data(:,t) = x;
% 	end
end


%% Reproduction with PGMM and DS-GMR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('New reproductions with DS-GMR...');
load('data/taskParams.mat'); %Load new task parameters (new situation)
for n=1:nbRepros
	rnew(n).p = taskParams(n).p;
	
Sylvain CALINON's avatar
Sylvain CALINON committed
146
% 	%Task parameters re-arranged as a vector (position and orientation)
147
% 	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];
Sylvain Calinon's avatar
Sylvain Calinon committed
148

Sylvain CALINON's avatar
Sylvain CALINON committed
149
	%Task parameters re-arranged as a vector (position only)
150 151 152 153
	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
Sylvain CALINON's avatar
Sylvain CALINON committed
154
		model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable
155 156 157
	end
	rnew(n).Mu = model.Mu;
	%Retrieval of attractor path through GMR
Sylvain CALINON's avatar
Sylvain CALINON committed
158
	[rnew(n).currTar, rnew(n).currSigma] = GMR(model, DataIn, 1, [2:model.nbVar]); 
159 160 161 162 163
	%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
Sylvain CALINON's avatar
Sylvain CALINON committed
164
		ddx =  -L * [x-rnew(n).currTar(:,t); dx]; 
165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
		dx = dx + ddx * model.dt;
		x = x + dx * model.dt;
		rnew(n).Data(:,t) = x;
	end
end


%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('PaperPosition',[0 0 4 3],'position',[20,50,600,450]);
axes('Position',[0 0 1 1]); axis off; hold on;
set(0,'DefaultAxesLooseInset',[0,0,0,0]);
limAxes = [-1.5 2.5 -1.6 1.4]*.8;
myclr = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078; 0.7412 0.0824 0.3137];

%Plot demonstrations
plotPegs(s(1).p(1), myclr(1,:), .1);
for n=1:nbSamples
	plotPegs(s(n).p(2), myclr(2,:), .1);
	patch([s(n).Data0(2,1:end) s(n).Data0(2,end:-1:1)], [s(n).Data0(3,1:end) s(n).Data0(3,end:-1:1)],...
		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
end
for n=1:nbSamples
	plotGMM(r(n).Mu(2:3,:),model.Sigma(2:3,2:3,:), [0 0 0], .04);
end
axis equal; axis(limAxes);
Sylvain Calinon's avatar
Sylvain Calinon committed
191
%print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png');
192 193

%Plot reproductions in new situations
Sylvain Calinon's avatar
Sylvain Calinon committed
194
disp('[Press enter to see next reproduction attempt]');
195 196 197 198 199 200 201 202 203 204
h=[];
for n=1:nbRepros
	delete(h);
	h = plotPegs(rnew(n).p);
	h = [h plotGMM(rnew(n).currTar, rnew(n).currSigma,  [0 .8 0], .2)];
	h = [h plotGMM(rnew(n).Mu(2:3,:), model.Sigma(2:3,2:3,:),  myclr(3,:), .6)];
	h = [h patch([rnew(n).Data(1,:) rnew(n).Data(1,fliplr(1:nbD))], [rnew(n).Data(2,:) rnew(n).Data(2,fliplr(1:nbD))],...
		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
	axis equal; axis(limAxes);
Sylvain Calinon's avatar
Sylvain Calinon committed
205 206
	%print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
	pause;
207 208 209 210 211 212 213 214 215
end

pause;
close all;
end

%Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa)
Sylvain CALINON's avatar
Sylvain CALINON committed
216 217 218 219 220 221 222 223 224 225
	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