demo_TPGMR_DS01.m 8.54 KB
Newer Older
Sylvain Calinon's avatar
Sylvain Calinon committed
1
function demo_TPGMR_DS01
2
% Task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
3 4 5
% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of
% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
% relevance of the frame through the task. This information is exploited to generate a new attractor path
Milad Malekzadeh's avatar
Milad Malekzadeh committed
6
% corresponding to new situations (new positions and orientation of the frames).
Sylvain Calinon's avatar
Sylvain Calinon committed
7 8
% This example presents the results for a time-based GMR reference retrieval process combined with a tracking system 
% with constant gains.
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
9
%
10 11 12 13
% 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.
Milad Malekzadeh's avatar
Milad Malekzadeh committed
14
%
15
% @article{Calinon16JIST,
16 17 18
%   author="Calinon, S.",
%   title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
%   journal="Intelligent Service Robotics",
19 20 21 22 23 24
%		publisher="Springer Berlin Heidelberg",
%		doi="10.1007/s11370-015-0187-9",
%		year="2016",
%		volume="9",
%		number="1",
%		pages="1--29"
25
% }
Milad Malekzadeh's avatar
Milad Malekzadeh committed
26
%
27
% @inproceedings{Calinon14,
Milad Malekzadeh's avatar
Milad Malekzadeh committed
28 29 30 31 32 33 34 35
%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
%   title="A task-parameterized probabilistic model with minimal intervention control",
%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
%   year="2014",
%   month="May-June",
%   address="Hong Kong, China",
%   pages="3339--3344"
% }
36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52
% 
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon (http://calinon.ch/) and Danilo Bruno (danilo.bruno@iit.it)
% 
% 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/>.
Milad Malekzadeh's avatar
Milad Malekzadeh committed
53

54 55
addpath('./m_fcts/');

Sylvain Calinon's avatar
Sylvain Calinon committed
56

Milad Malekzadeh's avatar
Milad Malekzadeh committed
57 58 59 60 61
%% 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)
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
62 63
model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain
64
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
65 66
nbRepros = 8; %Number of reproductions with new situations randomly generated

Sylvain Calinon's avatar
Sylvain Calinon committed
67

Milad Malekzadeh's avatar
Milad Malekzadeh committed
68 69 70
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...');
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
71 72 73 74
% 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
75 76
% 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).
Milad Malekzadeh's avatar
Milad Malekzadeh committed
77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
load('data/DataLQR01.mat');


%% Transformation of 'Data' to learn the path of the spring-damper system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbD = s(1).nbData;
nbVarOut = model.nbVar - 1;
%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));
%Create 3rd order tensor data with XHAT instead of X
for n=1:nbSamples
	DataTmp = s(n).Data0(2:end,:);
	DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
	for m=1:model.nbFrames
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
95
		Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
96 97 98 99
	end
end


100
%% TP-GMM learning
Milad Malekzadeh's avatar
Milad Malekzadeh committed
101
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102
fprintf('Parameters estimation of TP-GMM with EM:');
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
103
model = init_tensorGMM_timeBased(Data, model); %Initialization
Milad Malekzadeh's avatar
Milad Malekzadeh committed
104 105 106 107 108 109 110 111
model = EM_tensorGMM(Data, model);


%% Reproduction with DS-GMR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with DS-GMR...');
DataIn = [1:s(1).nbData] * model.dt;
for n=1:nbSamples
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
112 113
	%Retrieval of attractor path through task-parameterized GMR
	a(n) = estimateAttractorPath(DataIn, model, s(n));
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
114
	r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
115 116 117 118 119 120
end


%% Reproduction with DS-GMR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('New reproductions with DS-GMR...');
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
121
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
122
	for m=1:model.nbFrames
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
123
		%Random generation of new task parameters
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
124 125 126 127 128 129 130
		id=ceil(rand(2,1)*nbSamples);
		w=rand(2); w=w/sum(w);
		rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
		rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
	end
	%Retrieval of attractor path through task-parameterized GMR
	anew(n) = estimateAttractorPath(DataIn, model, rTmp);
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
131
	rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
132 133 134 135 136 137 138 139 140 141 142 143 144 145 146
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
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
147 148 149 150 151 152 153 154
	%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).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,:));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
155 156 157 158 159 160
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);

%REPROS
subplot(1,3,2); hold on; box on; title('Reproductions with DS-GMR');
for n=1:nbSamples
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
161 162 163 164 165
	%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
Milad Malekzadeh's avatar
Milad Malekzadeh committed
166 167
end
for n=1:nbSamples
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
168 169 170
	%Plot trajectories
	plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
	plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
171
end
172 173 174 175
for n=1:nbSamples
	%Plot Gaussians
	plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.5 .5 .5],.8);
end
Milad Malekzadeh's avatar
Milad Malekzadeh committed
176 177 178 179 180
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);

%NEW REPROS
subplot(1,3,3); hold on; box on; title('New reproductions with DS-GMR');
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
181 182 183 184 185
	%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
Milad Malekzadeh's avatar
Milad Malekzadeh committed
186 187
end
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
188 189 190
	%Plot trajectories
	plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
	plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
191
end
192 193 194 195
for n=1:nbRepros
	%Plot Gaussians
	plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.5 .5 .5],.8);
end
Milad Malekzadeh's avatar
Milad Malekzadeh committed
196 197
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);

198 199
%print('-dpng','graphs/demo_TPGMR_DS01.png');

Milad Malekzadeh's avatar
Milad Malekzadeh committed
200

Sylvain Calinon's avatar
Sylvain Calinon committed
201 202
%% Plot additional information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Milad Malekzadeh's avatar
Tab2.  
Milad Malekzadeh committed
203
figure;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
204 205 206
%Plot norm of control commands
subplot(1,2,1); hold on;
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
207
	plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
208 209 210 211 212
end
xlabel('t'); ylabel('|ddx|');
%Plot strength of the stiffness term
subplot(1,2,2); hold on;
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab1.  
Milad Malekzadeh committed
213
	plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
214 215 216 217 218 219 220
end
xlabel('t'); ylabel('|Kp|');

%pause;
%close all;