diff --git a/README.md b/README.md
index 1acd0ff4ee1ba49df152488b45fb96a6e40f89c8..eb387a1f06695dc0a03c0313446a16b6ea565eed 100644
--- a/README.md
+++ b/README.md
@@ -6,18 +6,18 @@
### Usage
- Unzip the file and run 'demo_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
- 'demo_TPGMR_DS01' (dynamical system with constant gains) in Matlab.
- 'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
+ Examples starting with 'demo_' can be run from Matlab/GNU Octave.
-### Reference
+### References
+
+ Calinon, S. (in preparation). A tutorial on task-parameterized movement learning and retrieval.
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description
- Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper systems
+ Demonstration of a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 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 corresponding to new situations (new positions and
@@ -25,10 +25,10 @@
estimate the stiffness and damping feedback terms of the spring-damper systems, resulting in a minimal intervention
control strategy.
-### Authors
+### Contact
- Sylvain Calinon and Danilo Bruno, 2014
- http://programming-by-demonstration.org/
+ Sylvain Calinon
+ http://programming-by-demonstration.org/lib/
This source code is given for free! In exchange, we would be grateful if you cite the following reference in any
academic publication that uses this code or part of it:
diff --git a/benchmark_DS_GP_GMM01.m b/benchmark_DS_GP_GMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..ab3b9025a7ffa877b1269d37025f8836be4f8e93
--- /dev/null
+++ b/benchmark_DS_GP_GMM01.m
@@ -0,0 +1,220 @@
+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
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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
+nbVarOut = model.nbVar-1;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+
+
+%% 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
+% 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 (5).
+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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+Data = s(1).Data0(1,:);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D];
+ Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
+end
+
+%% GPR with GMM encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of GPR with GMM encoding:');
+%GMM encoding for each trajectory sample, learned at once by stacking the different trajectory samples in 'Data' matrix of size M*D x T
+model.nbVar = size(Data,1); %Temporary modification of nbVar (for stacked data training)
+model = init_GMM_timeBased(Data, model);
+model = EM_GMM(Data, model);
+model.nbVar = size(s(1).p(1).A,1); %Setting back the initial nbVar
+for n=1:nbSamples
+ id = (n-1)*2+2:n*2+1;
+ s(n).Priors = model.Priors;
+ s(n).Mu = model.Mu([1,id],:);
+ s(n).Sigma = model.Sigma([1,id],[1,id],:);
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(s(n).Sigma(:,:,i));
+ U(:,:,i) = V * max(D,1E-3).^.5;
+ s(n).Sigma(:,:,i) = U(:,:,i) * U(:,:,i)';
+ end
+ %Set query point vector (position and orientation of the two objects)
+ s(n).DataIn = [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)];
+ model.DataIn(:,n) = s(n).DataIn;
+ %Set model output vector (Mu and Sigma)
+ model.DataOut(:,n) = [reshape(s(n).Mu, model.nbVar*model.nbStates, 1); reshape(s(n).Sigma, model.nbVar^2*model.nbStates, 1)];
+ %model.DataOut(:,n) = [reshape(s(n).Mu, model.nbVar*model.nbStates, 1); reshape(U, model.nbVar^2*model.nbStates, 1)];
+end
+
+
+%% Reproduction with GPR and 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
+ %Rebuild model parameters with GPR
+ vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn);
+
+ %Re-arrange GPR output as GMM parameters
+ r(n).Mu = reshape(vOut(1:model.nbVar*model.nbStates), model.nbVar, model.nbStates);
+ r(n).Sigma = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% U = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% for i=1:model.nbStates
+% r(n).Sigma(:,:,i) = U(:,:,i) * U(:,:,i)';
+% end
+ r(n).Priors = model.Priors;
+ r(n).nbStates = model.nbStates;
+
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(r(n).Sigma(:,:,i));
+ r(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
+ end
+
+% %Retrieval of attractor path through GMR
+% currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+%
+% %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
+% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+% dx = dx + ddx * model.dt;
+% x = x + dx * model.dt;
+% r(n).Data(:,t) = x;
+% end
+end
+
+
+%% Reproduction with GPR 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;
+ %Query point vector (position and orientation of the two objects)
+ rnew(n).DataIn = [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)];
+
+ %Rebuild model parameters with GPR
+ vOut = GPR(model.DataIn, model.DataOut, rnew(n).DataIn);
+
+ %Re-arrange GPR output as GMM parameters
+ rnew(n).Mu = reshape(vOut(1:model.nbVar*model.nbStates), model.nbVar, model.nbStates);
+ rnew(n).Sigma = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% U = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% for i=1:model.nbStates
+% rnew(n).Sigma(:,:,i) = U(:,:,i) * U(:,:,i)';
+% end
+ rnew(n).Priors = model.Priors;
+ rnew(n).nbStates = model.nbStates;
+
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(rnew(n).Sigma(:,:,i));
+ rnew(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
+ end
+
+ %Retrieval of attractor path through GMR
+ [rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+
+ %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
+ 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,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
+end
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png');
+
+%Plot reproductions in new situations
+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,:), rnew(n).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);
+ print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_GP_GMM02.m b/benchmark_DS_GP_GMM02.m
new file mode 100644
index 0000000000000000000000000000000000000000..7fbe93193c56e60c5fb38bcad86fe03abfc7f1ec
--- /dev/null
+++ b/benchmark_DS_GP_GMM02.m
@@ -0,0 +1,262 @@
+function benchmark_DS_GP_GMM02
+%Benchmark of task-parameterized model based on Gaussian process regression,
+%with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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 = 20; %Number of reproductions with new situations randomly generated
+nbVarOut = model.nbVar-1;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+
+
+%% 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
+% 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 (5).
+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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+Data = s(1).Data0(1,:);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D];
+ Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
+end
+
+%% GPR with GMM encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of GPR with GMM encoding:');
+%GMM encoding for each trajectory sample, learned at once by stacking the different trajectory samples in 'Data' matrix of size M*D x T
+model.nbVar = size(Data,1); %Temporary modification of nbVar (for stacked data training)
+model = init_GMM_timeBased(Data, model);
+model = EM_GMM(Data, model);
+model.nbVar = size(s(1).p(1).A,1); %Setting back the initial nbVar
+for n=1:nbSamples
+ id = (n-1)*2+2:n*2+1;
+ s(n).Priors = model.Priors;
+ s(n).Mu = model.Mu([1,id],:);
+ s(n).Sigma = model.Sigma([1,id],[1,id],:);
+% %Regularization of Sigma
+% for i=1:model.nbStates
+% [V,D] = eig(s(n).Sigma(:,:,i));
+% U(:,:,i) = V * max(D,1E-3).^.5;
+% s(n).Sigma(:,:,i) = U(:,:,i) * U(:,:,i)';
+% end
+ %Set query point vector (position and orientation of the two objects)
+ s(n).DataIn = [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)];
+ model.DataIn(:,n) = s(n).DataIn;
+ %Set model output vector (Mu and Sigma)
+ model.DataOut(:,n) = [reshape(s(n).Mu, model.nbVar*model.nbStates, 1); reshape(s(n).Sigma, model.nbVar^2*model.nbStates, 1)];
+ %model.DataOut(:,n) = [reshape(s(n).Mu, model.nbVar*model.nbStates, 1); reshape(U, model.nbVar^2*model.nbStates, 1)];
+end
+
+
+% %% Reproduction with GPR and 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
+% %Rebuild model parameters with GPR
+% vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn);
+%
+% %Re-arrange GPR output as GMM parameters
+% r(n).Mu = reshape(vOut(1:model.nbVar*model.nbStates), model.nbVar, model.nbStates);
+% r(n).Sigma = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% % U = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% % for i=1:model.nbStates
+% % r(n).Sigma(:,:,i) = U(:,:,i) * U(:,:,i)';
+% % end
+% r(n).Priors = model.Priors;
+% r(n).nbStates = model.nbStates;
+%
+% %Regularization of Sigma
+% for i=1:model.nbStates
+% [V,D] = eig(r(n).Sigma(:,:,i));
+% r(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
+% end
+%
+% % %Retrieval of attractor path through GMR
+% % currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+% %
+% % %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
+% % ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+% % dx = dx + ddx * model.dt;
+% % x = x + dx * model.dt;
+% % r(n).Data(:,t) = x;
+% % end
+% end
+
+
+%% Reproduction with GPR and DS-GMR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with DS-GMR...');
+
+% %Random generation of new task parameters
+% for n=1:10
+% id=ceil(rand(2,1)*nbSamples);
+% w=rand(2); w=w/sum(w);
+% taskParams(n).p(1) = s(1).p(1);
+% taskParams(n).p(2).b = s(id(1)).p(2).b * w(1) + s(id(2)).p(2).b * w(2);
+% taskParams(n).p(2).A = s(id(1)).p(2).A * w(1) + s(id(2)).p(2).A * w(2);
+% end
+% for n=11:20
+% taskParams(n).p(1) = s(1).p(1);
+% taskParams(n).p(2).b = [0; rand(1,1) * 2; rand(1,1)];
+% aTmp = rand(1) * pi + pi;
+% taskParams(n).p(2).A = [[1;0;0] [0;cos(aTmp);-sin(aTmp)] [0;sin(aTmp);cos(aTmp)]];
+% taskParams(n).p(2).A(2:end,2:end) = taskParams(n).p(2).A(2:end,2:end) * norm(s(1).p(1).A(:,2));
+% end
+% save('data/taskParams3.mat','taskParams');
+
+load('data/taskParams3.mat'); %Load new task parameters (new situation)
+
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbRepros
+ rnew(n).p = taskParams(n).p;
+ %Query point vector (position and orientation of the two objects)
+ rnew(n).DataIn = [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)];
+
+ %Rebuild model parameters with GPR
+ vOut = GPR(model.DataIn, model.DataOut, rnew(n).DataIn);
+
+ %Re-arrange GPR output as GMM parameters
+ rnew(n).Mu = reshape(vOut(1:model.nbVar*model.nbStates), model.nbVar, model.nbStates);
+ rnew(n).Sigma = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% U = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+% for i=1:model.nbStates
+% rnew(n).Sigma(:,:,i) = U(:,:,i) * U(:,:,i)';
+% end
+ rnew(n).Priors = model.Priors;
+ rnew(n).nbStates = model.nbStates;
+
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(rnew(n).Sigma(:,:,i));
+ rnew(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
+ end
+
+ %Retrieval of attractor path through GMR
+ [rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+
+ %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
+ 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 step-by-step
+plotPegs(s(1).p(1), myclr(1,:), .5);
+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
+axis equal; axis(limAxes);
+h=[];
+for n=1:nbSamples
+ delete(h)
+ h = plotPegs(s(n).p(2), myclr(2,:), .5);
+ h = [h plotGMM(s(n).Mu(2:3,:),s(n).Sigma(2:3,2:3,:), [0 0 0], 1)];
+ print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM_intro_step' num2str(n) 'a.png']);
+ h = [h plot2DArrow(s(n).p(2).b(2:3), s(n).p(2).A(2:3,3), [.8 0 0])];
+ print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM_intro_step' num2str(n) 'b.png']);
+end
+
+pause;
+close all;
+return;
+
+%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(s(n).Mu(2:3,:),s(n).Sigma(2:3,2:3,:), [0 0 0], .04);
+end
+axis equal; axis(limAxes);
+%print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM_intro00.png']);
+
+%Plot reproductions in new situations
+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,:), rnew(n).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);
+ print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM_intro' num2str(n,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_GP_raw01.m b/benchmark_DS_GP_raw01.m
new file mode 100644
index 0000000000000000000000000000000000000000..fc04165a065c58d5ebf6781ea036b896a95abad3
--- /dev/null
+++ b/benchmark_DS_GP_raw01.m
@@ -0,0 +1,164 @@
+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
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: 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
+L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV];
+
+%% 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
+% 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 (5).
+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(model.nbVar));
+%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+Data = s(1).Data0(1,:);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D];
+ Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
+end
+
+%% GPR with raw trajectory encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of GPR with raw trajectory encoding:');
+for n=1:nbSamples
+ %Set query point vector (position and orientation of the two objects)
+ s(n).DataIn = [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)];
+ model.DataIn(:,n) = s(n).DataIn;
+ %Set model output vector (raw trajectory data)
+ model.DataOut(:,n) = reshape(s(n).Data, model.nbVar*nbD, 1);
+end
+
+
+% %% Reproduction with GPR for the task parameters used to train the model
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% disp('Reproductions with spring-damper system...');
+% for n=1:nbSamples
+% %Direct retrieval of attractor path through GPR
+% vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn);
+% currTar = reshape(vOut, model.nbVar, nbD);
+%
+% %Motion retrieval with spring-damper system
+% x = s(n).p(1).b(2:3);
+% 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
+% dx = dx + ddx * model.dt;
+% x = x + dx * model.dt;
+% r(n).Data(:,t) = x;
+% end
+% end
+
+
+%% Reproduction with GPR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with spring-damper system...');
+load('data/taskParams.mat'); %Load new task parameters (new situation)
+for n=1:nbRepros
+ rnew(n).p = taskParams(n).p;
+ %Query point vector (position and orientation of the two objects)
+ rnew(n).DataIn = [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)];
+
+ %Direct retrieval of attractor path through GPR
+ [vOut, vOutSigma] = GPR(model.DataIn, model.DataOut, rnew(n).DataIn, [5E-1, 1E-1, 1E-2]);
+ rnew(n).currTar = reshape(vOut, model.nbVar, nbD);
+ for t=1:nbD
+ id = (t-1)*model.nbVar+1:t*model.nbVar;
+ %id = t:t+nbD:nbD*model.nbVar;
+ rnew(n).currSigma(:,:,t) = vOutSigma(id,id) / 20;
+ end
+
+ %Motion retrieval with spring-damper system
+ x = rnew(n).p(1).b(2:3);
+ 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
+ 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
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png');
+
+%Plot reproductions in new situations
+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 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);
+ print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_GP_raw02.m b/benchmark_DS_GP_raw02.m
new file mode 100644
index 0000000000000000000000000000000000000000..68e6ab144aa832af98b997ebf8c95cbfa0f4df32
--- /dev/null
+++ b/benchmark_DS_GP_raw02.m
@@ -0,0 +1,183 @@
+function benchmark_DS_GP_raw02
+%Benchmark of task-parameterized model based on Gaussian process regression,
+%with raw trajectory, and spring-damper system used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: 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 = 20; %Number of reproductions with new situations randomly generated
+L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV];
+
+%% 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
+% 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 (5).
+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(model.nbVar));
+%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+Data = s(1).Data0(1,:);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D];
+ Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
+end
+
+%% GPR with raw trajectory encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of GPR with raw trajectory encoding:');
+for n=1:nbSamples
+ %Set query point vector (position and orientation of the two objects)
+ s(n).DataIn = [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)];
+ model.DataIn(:,n) = s(n).DataIn;
+ %Set model output vector (raw trajectory data)
+ model.DataOut(:,n) = reshape(s(n).Data, model.nbVar*nbD, 1);
+end
+
+
+% %% Reproduction with GPR for the task parameters used to train the model
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% disp('Reproductions with spring-damper system...');
+% for n=1:nbSamples
+% %Direct retrieval of attractor path through GPR
+% vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn);
+% currTar = reshape(vOut, model.nbVar, nbD);
+%
+% %Motion retrieval with spring-damper system
+% x = s(n).p(1).b(2:3);
+% 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
+% dx = dx + ddx * model.dt;
+% x = x + dx * model.dt;
+% r(n).Data(:,t) = x;
+% end
+% end
+
+
+%% Reproduction with GPR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with spring-damper system...');
+
+% %Random generation of new task parameters
+% for n=1:10
+% id=ceil(rand(2,1)*nbSamples);
+% w=rand(2); w=w/sum(w);
+% taskParams(n).p(1) = s(1).p(1);
+% taskParams(n).p(2).b = s(id(1)).p(2).b * w(1) + s(id(2)).p(2).b * w(2);
+% taskParams(n).p(2).A = s(id(1)).p(2).A * w(1) + s(id(2)).p(2).A * w(2);
+% end
+% for n=11:20
+% taskParams(n).p(1) = s(1).p(1);
+% taskParams(n).p(2).b = [0; rand(1,1) * 2; rand(1,1)];
+% aTmp = rand(1) * pi + pi;
+% taskParams(n).p(2).A = [[1;0;0] [0;cos(aTmp);-sin(aTmp)] [0;sin(aTmp);cos(aTmp)]];
+% taskParams(n).p(2).A(2:end,2:end) = taskParams(n).p(2).A(2:end,2:end) * norm(s(1).p(1).A(:,2));
+% end
+% save('data/taskParams2.mat','taskParams');
+
+load('data/taskParams2.mat'); %Load new task parameters (new situation)
+
+
+for n=1:nbRepros
+ rnew(n).p = taskParams(n).p;
+ %Query point vector (position and orientation of the two objects)
+ rnew(n).DataIn = [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)];
+
+ %Direct retrieval of attractor path through GPR
+ [vOut, vOutSigma] = GPR(model.DataIn, model.DataOut, rnew(n).DataIn, [5E-1, 1E-1, 1E-2]);
+ rnew(n).currTar = reshape(vOut, model.nbVar, nbD);
+ for t=1:nbD
+ id = (t-1)*model.nbVar+1:t*model.nbVar;
+ %id = t:t+nbD:nbD*model.nbVar;
+ rnew(n).currSigma(:,:,t) = vOutSigma(id,id) / 20;
+ end
+
+ %Motion retrieval with spring-damper system
+ x = rnew(n).p(1).b(2:3);
+ 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
+ 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
+axis equal; axis(limAxes);
+
+%Plot reproductions in new situations
+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 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);
+ print('-dpng','-r600',['graphs/benchmark_DS_GP_raw_intro' num2str(n,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_PGMM01.m b/benchmark_DS_PGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..6aaf9df7819cafd0e629d3f2162791627c689ebd
--- /dev/null
+++ b/benchmark_DS_PGMM01.m
@@ -0,0 +1,192 @@
+function benchmark_DS_PGMM01
+%Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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
+nbVarOut = model.nbVar - 1;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+
+
+%% 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
+% 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 (5).
+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));
+%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+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
+% %Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+% 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. (7.1.4) in doc/TechnicalReport.pdf
+ 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);
+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);
+ %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
+ model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf
+ end
+ r(n).Mu = model.Mu;
+% %Retrieval of attractor path through GMR
+% currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+% %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
+% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+% 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;
+
+% %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+% 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
+ 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
+ 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
+ %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
+ 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);
+print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png');
+
+%Plot reproductions in new situations
+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);
+ print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_TP_GMM01.m b/benchmark_DS_TP_GMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..b0df6d1d95db7246bb0b2a19ba5b97d95f7ff4ab
--- /dev/null
+++ b/benchmark_DS_TP_GMM01.m
@@ -0,0 +1,137 @@
+function benchmark_DS_TP_GMM01
+%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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
+
+
+%% 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
+% 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 (5).
+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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+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
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
+ end
+end
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+%model = init_tensorGMM_kmeans(Data, model); %Initialization
+model = init_tensorGMM_timeBased(Data, model); %Initialization
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with DS-GMR...');
+DataIn = [1:nbD] * model.dt;
+for n=1:nbSamples
+ %Retrieval of attractor path through task-parameterized GMR
+ a(n) = estimateAttractorPath(DataIn, model, s(n));
+ r(n) = reproduction_DS(DataIn, model, a(n), s(n).p(1).b(2:3));
+end
+
+
+%% Reproduction for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with DS-GMR...');
+load('data/taskParams.mat'); %Load new task parameters (new situation)
+for n=1:nbRepros
+ %Retrieval of attractor path through task-parameterized GMR
+ anew(n) = estimateAttractorPath(DataIn, model, taskParams(n));
+ rnew(n) = reproduction_DS(DataIn, model, anew(n), taskParams(n).p(1).b(2:3));
+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,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
+end
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png');
+
+%Plot reproductions in new situations
+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_GMM' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_TP_GP01.m b/benchmark_DS_TP_GP01.m
new file mode 100644
index 0000000000000000000000000000000000000000..a0714527dccb791488532a1f0722c9a394f76658
--- /dev/null
+++ b/benchmark_DS_TP_GP01.m
@@ -0,0 +1,160 @@
+function benchmark_DS_TP_GP01
+%Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+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
+nbVarOut = model.nbVar-1;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+
+
+%% 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
+% 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 (5).
+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(model.nbVar-1));
+%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
+ for m=1:model.nbFrames
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (s(n).Data - repmat(s(n).p(m).b, 1, nbD));
+ s(n).p(m).A = s(n).p(m).A(2:end,2:end);
+ s(n).p(m).b = s(n).p(m).b(2:end);
+ end
+end
+
+
+%% Reproduction with TP-GP for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with TP-GP and spring-damper system...');
+in=1; out=2:model.nbVar;
+model.nbVar = model.nbVar-1;
+model.nbStates = nbD;
+model.Priors = ones(model.nbStates,1) / model.nbStates;
+for m=1:model.nbFrames
+ DataIn(1,:) = squeeze(Data(in,m,:));
+ DataOut = squeeze(Data(out,m,:));
+ [MuTmp, SigmaTmp] = GPR(DataIn, DataOut, DataIn(1,1:nbD), [1E1, 1E2, 1E0]);
+ model.Mu(:,m,:) = MuTmp;
+ model.Sigma(:,:,m,:) = SigmaTmp;
+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
+%
+% %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
+% dx = dx + ddx * model.dt;
+% x = x + dx * model.dt;
+% r(n).Data(:,t) = x;
+% end
+% end
+
+
+%% Reproduction with TP-GP for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with TP-GP and spring-damper system...');
+load('data/taskParams.mat'); %Load new task parameters (new situation)
+for n=1:nbRepros
+ for m=1:model.nbFrames
+ 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
+
+ %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
+ 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
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png');
+
+%Plot reproductions in new situations
+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 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);
+ print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_TP_LWR01.m b/benchmark_DS_TP_LWR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..e7f66818f2c640184bf015a6b5e2c8c09f5b21e3
--- /dev/null
+++ b/benchmark_DS_TP_LWR01.m
@@ -0,0 +1,171 @@
+function benchmark_DS_TP_LWR01
+%Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+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
+nbVarOut = model.nbVar-1;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+
+
+%% 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
+% 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 (5).
+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(model.nbVar-1));
+%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
+ for m=1:model.nbFrames
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (s(n).Data - repmat(s(n).p(m).b, 1, nbD));
+ s(n).p(m).A = s(n).p(m).A(2:end,2:end);
+ s(n).p(m).b = s(n).p(m).b(2:end);
+ end
+end
+
+
+%% Reproduction with TP-GP for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with TP-GP and spring-damper system...');
+in=1; out=2:model.nbVar;
+model.nbVar = model.nbVar-1;
+model.nbStates = nbD;
+model.Priors = ones(model.nbStates,1) / model.nbStates;
+for m=1:model.nbFrames
+ DataIn(1,:) = squeeze(Data(in,m,:));
+ DataOut = squeeze(Data(out,m,:));
+ for t=1:nbD
+ W(:,t) = gaussPDF(DataIn(1,:), DataIn(1,t), eye(length(in)) * 1E-3);
+ W(:,t) = W(:,t) / sum(W(:,t));
+ end
+ %LWR
+ MuTmp = DataOut * W;
+ %Evaluate Sigma
+ SigmaTmp = zeros(model.nbVar, model.nbVar, nbD);
+ for t=1:nbD
+ ym = repmat(MuTmp(:,t),1,nbD*nbSamples);
+ SigmaTmp(:,:,t) = (DataOut-ym) * diag(W(:,t)) * (DataOut-ym)'; %+ 1E-2*eye(2)
+ end
+ model.Mu(:,m,:) = MuTmp;
+ model.Sigma(:,:,m,:) = SigmaTmp;
+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
+%
+% %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
+% dx = dx + ddx * model.dt;
+% x = x + dx * model.dt;
+% r(n).Data(:,t) = x;
+% end
+% end
+
+
+%% Reproduction with TP-GP for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with TP-GP and spring-damper system...');
+load('data/taskParams.mat'); %Load new task parameters (new situation)
+for n=1:nbRepros
+ for m=1:model.nbFrames
+ 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
+
+ %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
+ 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
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png');
+
+%Plot reproductions in new situations
+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 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);
+ print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/benchmark_DS_TP_MFA01.m b/benchmark_DS_TP_MFA01.m
new file mode 100644
index 0000000000000000000000000000000000000000..92be712e06a149f6b8a2f9c280ec5d31dfb196d0
--- /dev/null
+++ b/benchmark_DS_TP_MFA01.m
@@ -0,0 +1,138 @@
+function benchmark_DS_TP_MFA01
+%Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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.nbFA = 1; %Dimension of factor analyzers
+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
+
+
+%% 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
+% 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 (5).
+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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+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
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
+ end
+end
+
+
+%% Tensor MFA learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+%model = init_tensorGMM_kmeans(Data, model); %Initialization
+model = init_tensorGMM_timeBased(Data, model); %Initialization
+model = EM_tensorMFA(Data, model);
+
+
+%% Reproduction for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with DS-GMR...');
+DataIn = [1:nbD] * model.dt;
+for n=1:nbSamples
+ %Retrieval of attractor path through task-parameterized GMR
+ a(n) = estimateAttractorPath(DataIn, model, s(n));
+ r(n) = reproduction_DS(DataIn, model, a(n), s(n).p(1).b(2:3));
+end
+
+
+%% Reproduction for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with DS-GMR...');
+load('data/taskParams.mat'); %Load new task parameters (new situation)
+for n=1:nbRepros
+ %Retrieval of attractor path through task-parameterized GMR
+ anew(n) = estimateAttractorPath(DataIn, model, taskParams(n));
+ rnew(n) = reproduction_DS(DataIn, model, anew(n), taskParams(n).p(1).b(2:3));
+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,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
+end
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_TP_MFA01.png');
+
+%Plot reproductions in new situations
+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
+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
+
+
+
diff --git a/benchmark_DS_TP_trajGMM01.m b/benchmark_DS_TP_trajGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..9078fe8e876797b1d950c3dfb63a3e6fda2d4add
--- /dev/null
+++ b/benchmark_DS_TP_trajGMM01.m
@@ -0,0 +1,255 @@
+function benchmark_DS_TP_trajGMM01
+%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
+model.dt = .1; %Time step
+model.kP = 10; %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
+L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV];
+
+
+%% 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
+% 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 (5).
+load('data/DataLQR01.mat');
+
+
+%% Transformation of Data to learn the path of the spring-damper system instead of the raw data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+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(model.nbVarPos));
+%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
+end
+
+% for n=1:nbSamples
+% s(n).Data = s(n).Data0;
+% end
+
+
+%% Compute derivatives
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+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 3rd order tensor data with XHAT instead of X
+model.nbVar = model.nbVarPos * model.nbDeriv;
+Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
+DataT = zeros(model.nbVar+1, model.nbFrames, nbD*nbSamples);
+for n=1:nbSamples
+ DataTmp = s(n).Data(2:end,:);
+ for k=1:model.nbDeriv-1
+ DataTmp = [DataTmp; s(n).Data(2:end,:)*D^k]; %Compute derivatives
+ end
+ for m=1:model.nbFrames
+ s(n).p(m).b = [s(n).p(m).b(2:end); zeros((model.nbDeriv-1)*model.nbVarPos,1)];
+ s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A(2:end,2:end));
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
+ DataT(:,m,(n-1)*nbD+1:n*nbD) = [1:nbD; squeeze(Data(:,m,(n-1)*nbD+1:n*nbD))];
+ end
+end
+
+
+%% Construct operator PHI (big sparse matrix), see Eq. (2.4.5) in doc/TechnicalReport.pdf
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+T1 = nbD; %Number of datapoints in a demonstration
+T = T1 * nbSamples; %Total number of datapoints
+op1D = zeros(model.nbDeriv);
+op1D(1,end) = 1;
+for i=2:model.nbDeriv
+ op1D(i,:) = (op1D(i-1,:) - circshift(op1D(i-1,:),[0,-1])) / model.dt;
+end
+op = zeros(T1*model.nbDeriv,T1);
+op((model.nbDeriv-1)*model.nbDeriv+1:model.nbDeriv*model.nbDeriv,1:model.nbDeriv) = op1D;
+PHI0 = zeros(T1*model.nbDeriv,T1);
+for t=0:T1-model.nbDeriv
+ PHI0 = PHI0 + circshift(op, [model.nbDeriv*t,t]);
+end
+%Handling of borders
+for i=1:model.nbDeriv-1
+ op(model.nbDeriv*model.nbDeriv+1-i,:)=0; op(:,i)=0;
+ PHI0 = PHI0 + circshift(op, [-i*model.nbDeriv,-i]);
+end
+%Application to multiple dimensions and multiple demonstrations
+PHI1 = kron(PHI0, eye(model.nbVarPos));
+PHI = kron(eye(nbSamples), PHI1);
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+
+% %k-means initialization
+% model = init_tensorGMM_kmeans(Data, model);
+
+%Time-based initialization
+modelT = model;
+modelT.nbVar = model.nbVar+1;
+modelT = init_tensorGMM_timeBased(DataT, modelT);
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ model.Mu(:,m,i) = modelT.Mu(2:end,m,i);
+ model.Sigma(:,:,m,i) = modelT.Sigma(2:end,2:end,m,i);
+ end
+end
+model.Priors = modelT.Priors;
+
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions...');
+%DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+ %Products of linearly transformed Gaussians
+ for i=1:model.nbStates
+ SigmaTmp = zeros(model.nbVar);
+ MuTmp = zeros(model.nbVar,1);
+ for m=1:model.nbFrames
+ MuP = s(n).p(m).A * model.Mu(:,m,i) + s(n).p(m).b;
+ SigmaP = s(n).p(m).A * model.Sigma(:,:,m,i) * s(n).p(m).A';
+ SigmaTmp = SigmaTmp + inv(SigmaP);
+ MuTmp = MuTmp + SigmaP\MuP;
+ end
+ r(n).Sigma(:,:,i) = inv(SigmaTmp);
+ r(n).Mu(:,i) = r(n).Sigma(:,:,i) * MuTmp;
+ end
+end
+
+
+%% Reproduction for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions...');
+load('data/taskParams.mat'); %Load new task parameters (new situation)
+for n=1:nbRepros
+ %Adapt task parameters to trajectory GMM
+ for m=1:model.nbFrames
+ rnew(n).p(m).b = [taskParams(n).p(m).b(2:end); zeros((model.nbDeriv-1)*model.nbVarPos,1)];
+ rnew(n).p(m).A = kron(eye(model.nbDeriv), taskParams(n).p(m).A(2:end,2:end));
+ end
+ %GMM products
+ for i=1:model.nbStates
+ SigmaTmp = zeros(model.nbVar);
+ MuTmp = zeros(model.nbVar,1);
+ for m=1:model.nbFrames
+ MuP = rnew(n).p(m).A * model.Mu(:,m,i) + rnew(n).p(m).b;
+ SigmaP = rnew(n).p(m).A * model.Sigma(:,:,m,i) * rnew(n).p(m).A';
+ SigmaTmp = SigmaTmp + inv(SigmaP);
+ MuTmp = MuTmp + SigmaP\MuP;
+ end
+ rnew(n).Sigma(:,:,i) = inv(SigmaTmp);
+ rnew(n).Mu(:,i) = rnew(n).Sigma(:,:,i) * MuTmp;
+ end
+ %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q
+ %[~,rnew(n).q] = max(model.Pix(:,3*T1+1:4*T1),[],1); %works also for nbStates=1
+ mPix = mean(reshape(model.Pix,[model.nbStates, nbD, nbSamples]),3); %Average Pix
+ [~,rnew(n).q] = max(mPix,[],1); %works also for nbStates=1
+ rnew(n).MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVarPos*model.nbDeriv*T1, 1);
+ rnew(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1);
+ for t=1:T1
+ id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv;
+ rnew(n).SigmaQ(id1,id1) = rnew(n).Sigma(:,:,rnew(n).q(t));
+ end
+ %Retrieval of data with weighted least squares solution
+ [zeta,~,~,S] = lscov(PHI1, rnew(n).MuQ, rnew(n).SigmaQ,'chol');
+
+ %Reproduction without DS
+ %rnew(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data
+
+ %Reproduction with DS
+ rnew(n).currTar = reshape(zeta, model.nbVarPos, T1); %Reshape data
+ x = rnew(n).p(1).b(1:2);
+ dx = zeros(model.nbVarPos,1);
+ for t=1:nbD
+ ddx = -L * [x-rnew(n).currTar(:,t); dx];
+ dx = dx + ddx * model.dt;
+ x = x + dx * model.dt;
+ rnew(n).Data(:,t) = x;
+ %Rebuild covariance by reshaping S, see Eq. (2.4.12) in doc/TechnicalReport.pdf
+ id = (t-1)*model.nbVarPos+1:t*model.nbVarPos;
+ rnew(n).currSigma(:,:,t) = S(id,id) * nbD;
+ 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(1:2,:),r(n).Sigma(1:2,1:2,:), [0 0 0], .04);
+end
+axis equal; axis(limAxes);
+print('-dpng','-r600','graphs/benchmark_DS_TP_trajGMM01.png');
+
+%Plot reproductions in new situations
+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(1:2,:), rnew(n).Sigma(1:2,1:2,:), 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);
+ print('-dpng','-r600',['graphs/benchmark_DS_TP_trajGMM' num2str(n+1,'%.2d') '.png']);
+ %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
+
+
+
diff --git a/data/Data01.mat b/data/Data01.mat
new file mode 100644
index 0000000000000000000000000000000000000000..058003757394204888eed395482085b9d3b2ba62
Binary files /dev/null and b/data/Data01.mat differ
diff --git a/data/Data02.mat b/data/Data02.mat
new file mode 100644
index 0000000000000000000000000000000000000000..63b6a7048cf92f4b3770926b1862fbf5d7075d4d
Binary files /dev/null and b/data/Data02.mat differ
diff --git a/data/TPGMMpointing_nullspace_data02.mat b/data/TPGMMpointing_nullspace_data02.mat
new file mode 100644
index 0000000000000000000000000000000000000000..ec8e57e3ca33d94c3b66019d2756614fb7a5af63
Binary files /dev/null and b/data/TPGMMpointing_nullspace_data02.mat differ
diff --git a/data/TPGMMpointing_nullspace_model02.mat b/data/TPGMMpointing_nullspace_model02.mat
new file mode 100644
index 0000000000000000000000000000000000000000..3de1f30862e922a00fef5da5ddf7917081c7e6f3
Binary files /dev/null and b/data/TPGMMpointing_nullspace_model02.mat differ
diff --git a/data/TPGMMpointing_nullspace_repro02.mat b/data/TPGMMpointing_nullspace_repro02.mat
new file mode 100644
index 0000000000000000000000000000000000000000..0c1cb16c33353933c22864c7ff9fc71018da3972
Binary files /dev/null and b/data/TPGMMpointing_nullspace_repro02.mat differ
diff --git a/data/TPGMMtmp.mat b/data/TPGMMtmp.mat
new file mode 100644
index 0000000000000000000000000000000000000000..157c0378aaee45aa2246073737837c9838daeb20
Binary files /dev/null and b/data/TPGMMtmp.mat differ
diff --git a/data/dataTrajGMM01.mat b/data/dataTrajGMM01.mat
new file mode 100644
index 0000000000000000000000000000000000000000..fa49c0034f9fcb9a27b1125e629170108da8f8b4
Binary files /dev/null and b/data/dataTrajGMM01.mat differ
diff --git a/data/dataTrajGMM02.mat b/data/dataTrajGMM02.mat
new file mode 100644
index 0000000000000000000000000000000000000000..553c28f0370ab4059ce57a951377f0fa53563340
Binary files /dev/null and b/data/dataTrajGMM02.mat differ
diff --git a/data/dataTrajGMM03.mat b/data/dataTrajGMM03.mat
new file mode 100644
index 0000000000000000000000000000000000000000..56a7750a7d58803a7036349779269bd38d6984e5
Binary files /dev/null and b/data/dataTrajGMM03.mat differ
diff --git a/data/dataTrajGMM04.mat b/data/dataTrajGMM04.mat
new file mode 100644
index 0000000000000000000000000000000000000000..fa0c56e175089b65b1d6ca396fc88ab43987a7ae
Binary files /dev/null and b/data/dataTrajGMM04.mat differ
diff --git a/demoIK_nullspace_TPGMM01.m b/demoIK_nullspace_TPGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..01ea7fa0058eae3bd2182080c1236b5d2b52e573
--- /dev/null
+++ b/demoIK_nullspace_TPGMM01.m
@@ -0,0 +1,238 @@
+function demoIK_nullspace_TPGMM01
+%IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames)
+%First run 'startup_rvc' from the robotics toolbox
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 1; %Number of states
+model.nbFrames = 4; %Number of frames
+model.nbVars = [2,2,2,2]; %[xl],[xr2],[xl2],[xr]
+model.nbVar = max(model.nbVars);
+model.nbQ = 5; %Number of variables in configuration space (joint angles)
+model.nbX = 2; %Number of variables in operational space (end-effector position)
+model.nbVarOut = model.nbQ; %[q]
+model.dt = 0.01; %Time step
+nbSamples = 1; %Number of demonstration
+nbRepros = 1; %Number of reproduction
+nbData = 200; %Number of datapoints in a demonstration
+pinvDampCoeff = 1e-8; %Coefficient for damped pseudoinverse
+
+needsModel = 1;
+
+%% Create robot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+armLength = .5;
+L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
+arm = SerialLink(repmat(L1,3,1));
+
+%% Generate demonstrations
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+if needsModel==1
+disp('Demonstration...');
+for n=1:nbSamples
+ q = [pi/2 pi/2 pi/3 -pi/2 -pi/3]'; %Initial pose
+ for t=1:nbData
+ s(n).q(:,t) = q; %Log data
+ %Forward kinematics
+ Htmp = arm.fkine(q(1:3));
+ s(n).lx(:,t) = Htmp(1:2,end);
+ Htmp = arm.fkine(q([1,4:5]));
+ s(n).rx(:,t) = Htmp(1:2,end);
+ %Reference trajectory
+ if t==1
+ %Objects moving on a line
+ s(n).lxh = [linspace(s(n).lx(1,1),s(n).lx(1,1)-.6*armLength,nbData); linspace(s(n).lx(2,1),s(n).lx(2,1)+2*armLength,nbData)];
+ s(n).rxh = [linspace(s(n).rx(1,1),s(n).rx(1,1)+.6*armLength,nbData); linspace(s(n).rx(2,1),s(n).rx(2,1)+2*armLength,nbData)];
+ end
+ %Build Jacobians
+ lJ = arm.jacob0(q(1:3),'trans');
+ lJ = lJ(1:2,:);
+ rJ = arm.jacob0(q([1,4:5]),'trans');
+ rJ = rJ(1:2,:);
+ J = lJ;
+ J(3:4,[1,4:5]) = rJ;
+ Ja = J(1:2,:);
+ Jb = J(3:4,:);
+ pinvJ = (J'*J+eye(model.nbQ)*pinvDampCoeff) \ J'; %damped pseudoinverse
+ pinvJa = (Ja'*Ja+eye(model.nbQ)*pinvDampCoeff) \ Ja'; %damped pseudoinverse
+ pinvJb = (Jb'*Jb+eye(model.nbQ)*pinvDampCoeff) \ Jb'; %damped pseudoinverse
+
+% Na = eye(model.nbQ) - pinvJa*Ja; %Nullspace projection matrix
+% Nb = eye(model.nbQ) - pinvJb*Jb; %Nullspace projection matrix
+
+ %An alternative way of computing the nullspace projection matrix is given by
+ %http://math.stackexchange.com/questions/421813/projection-matrix-onto-null-space
+ [U,S,V] = svd(pinvJa);
+ S2 = zeros(model.nbQ);
+ S2(model.nbX+1:end,model.nbX+1:end) = eye(model.nbQ-model.nbX);
+ Na = U * S2 * U';
+% %pinvNa = U*(eye(5)-S2)*U';
+% pinvNa = U*pinv(S2)*U';
+
+ [U,S,V] = svd(pinvJb);
+ S2 = zeros(model.nbQ);
+ S2(model.nbX+1:end,model.nbX+1:end) = eye(model.nbQ-model.nbX);
+ Nb = U * S2 * U';
+% %pinvNb = U*(eye(5)-S2)*U';
+% pinvNb = U*pinv(S2)*U';
+
+ %IK controller
+ ldx = (s(n).lxh(:,t) - s(n).lx(:,t)) / model.dt;
+ rdx = (s(n).rxh(:,t) - s(n).rx(:,t)) / model.dt;
+
+ %Generate artificial dataset
+ %dq = pinvJ * [ldx; rdx]; %Equal priority between arms
+ dq = pinvJa * ldx + Na * pinvJb * rdx; %Priority on left arm
+ %dq = pinvJb * rdx + Nb * pinvJa * ldx; %Priority on right arm
+
+ s(n).fr(1).Data(:,t) = ldx * model.dt;
+ s(n).fr(2).Data(:,t) = Jb*Na*pinvJb*rdx * model.dt;
+ s(n).fr(3).Data(:,t) = Ja*Nb*pinvJa*ldx * model.dt;
+ s(n).fr(4).Data(:,t) = rdx * model.dt;
+
+ q = q + dq * model.dt;
+ end
+end
+
+
+%% Create dataset
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Data = zeros(model.nbVar, model.nbFrames, nbData*nbSamples);
+for n=1:nbSamples
+ s(n).nbData = nbData;
+ for m=1:model.nbFrames
+ Data(1:model.nbVars(m),m,(n-1)*nbData+1:n*nbData) = s(n).fr(m).Data;
+ end
+end
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-GMM with EM:');
+model = init_TPGMM_timeBased(Data, model); %Initialization
+%model = init_TPGMM_kmeans(Data, model); %Initialization
+model = EM_TPGMM(Data, model);
+
+model.nbVar = model.nbQ; %Update nbVar to use productTPGMM()
+
+
+%% Reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproduction...');
+rr.Priors = model.Priors;
+rr.nbStates = model.nbStates;
+for n=1:nbRepros
+ r(n).q(:,1) = [pi/2 pi/2 pi/3 -pi/2 -pi/3]; %Initial pose
+ for t=1:nbData
+ %Forward kinematics
+ Htmp = arm.fkine(r(n).q(1:3,t));
+ r(n).lx(:,t) = Htmp(1:2,end);
+ Htmp = arm.fkine(r(n).q([1,4:5],t));
+ r(n).rx(:,t) = Htmp(1:2,end);
+ %Reference trajectory
+ if t==1
+ %Objects moving on a line
+ r(n).lxh = [linspace(r(n).lx(1,1),r(n).lx(1,1)-.6*armLength,nbData); linspace(r(n).lx(2,1),r(n).lx(2,1)+2*armLength,nbData)];
+ r(n).rxh = [linspace(r(n).rx(1,1),r(n).rx(1,1)+.6*armLength,nbData); linspace(r(n).rx(2,1),r(n).rx(2,1)+2*armLength,nbData)];
+ end
+ %IK controller
+ ldx = (r(n).lxh(:,t) - r(n).lx(:,t)) / model.dt;
+ rdx = (r(n).rxh(:,t) - r(n).rx(:,t)) / model.dt;
+ %Build Jacobians
+ lJ = arm.jacob0(r(n).q(1:3,t),'trans');
+ lJ = lJ(1:2,:);
+ rJ = arm.jacob0(r(n).q([1,4:5],t),'trans');
+ rJ = rJ(1:2,:);
+ J = lJ;
+ J(3:4,[1,4:5]) = rJ;
+ Ja = J(1:2,:);
+ Jb = J(3:4,:);
+ pinvJa = (Ja'*Ja+eye(model.nbQ)*pinvDampCoeff) \ Ja'; %damped pseudoinverse
+ pinvJb = (Jb'*Jb+eye(model.nbQ)*pinvDampCoeff) \ Jb'; %damped pseudoinverse
+
+% Na = eye(model.nbQ) - pinvJa*Ja; %Nullspace projection matrix
+% Nb = eye(model.nbQ) - pinvJb*Jb; %Nullspace projection matrix
+
+ %An alternative way of computing the nullspace projection matrix is given by
+ %http://math.stackexchange.com/questions/421813/projection-matrix-onto-null-space
+ [U,S,V] = svd(pinvJa);
+ S2 = zeros(model.nbQ);
+ S2(model.nbX+1:end,model.nbX+1:end) = eye(model.nbQ-model.nbX);
+ Na = U * S2 * U';
+% %pinvNa = U*(eye(5)-S2)*U';
+% pinvNa = U*pinv(S2)*U';
+
+ [U,S,V] = svd(pinvJb);
+ S2 = zeros(model.nbQ);
+ S2(model.nbX+1:end,model.nbX+1:end) = eye(model.nbQ-model.nbX);
+ Nb = U * S2 * U';
+% %pinvNb = U*(eye(5)-S2)*U';
+% pinvNb = U*pinv(S2)*U';
+
+ %Update frames
+ %Priority on left arm (dq = pinvJa * ldx + Na * pinvJb * rdx)
+ %left
+ pTmp(1).A = pinvJa;
+ pTmp(1).b = r(n).q(:,t) + pinvJa * ldx * model.dt;
+ %right
+ pTmp(2).A = Na * pinvJb;
+ pTmp(2).b = r(n).q(:,t) + Na * pinvJb * rdx * model.dt;
+ %Priority on right arm (dq = pinvJb * rdx + Nb * pinvJa * ldx)
+ %left
+ pTmp(3).A = Nb * pinvJa;
+ pTmp(3).b = r(n).q(:,t) + Nb * pinvJa * ldx * model.dt;
+ %right
+ pTmp(4).A = pinvJb;
+ pTmp(4).b = r(n).q(:,t) + pinvJb * rdx * model.dt;
+
+ %Reproduction with TPGMM
+ [rr.Mu, rr.Sigma] = productTPGMM(model, pTmp);
+ r(n).q(:,t+1) = rr.Mu;
+ end
+end
+
+save('data/TPGMMtmp.mat','s','r','model');
+end %needsModel
+load('data/TPGMMtmp.mat');
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,450],'color',[1 1 1]);
+colTmp = repmat(linspace(.8,.2,nbData),3,1)';
+
+%DEMOS
+subplot(1,2,1); hold on; axis off; title('Demonstration');
+for n=1:nbSamples
+ for t=round(linspace(1,nbData,10))
+ plotArm(s(n).q(1:3,t), ones(3,1)*armLength, [0;0;t/nbData], .02, colTmp(t,:)); %left arm
+ plotArm(s(n).q([1,4:5],t), ones(3,1)*armLength, [0;0;t/nbData], .02, colTmp(t,:)); %right arm
+ end
+end
+for n=1:nbSamples
+ plot3(s(n).rxh(1,:), s(n).rxh(2,:), ones(1,nbData)*2, 'r-','linewidth',2);
+ plot3(s(n).lxh(1,:), s(n).lxh(2,:), ones(1,nbData)*2, 'r-','linewidth',2);
+end
+set(gca,'xtick',[],'ytick',[]); axis equal; %axis([-1.1 1.1 -.1 1.2]);
+
+%REPROS
+subplot(1,2,2); hold on; axis off; title('Reproduction');
+for n=1:nbRepros
+ for t=round(linspace(1,nbData,10))
+ plotArm(r(n).q(1:3,t), ones(3,1)*armLength, [0;0;t/nbData], .02, colTmp(t,:)); %left arm
+ plotArm(r(n).q([1,4:5],t), ones(3,1)*armLength, [0;0;t/nbData], .02, colTmp(t,:)); %right arm
+ end
+end
+for n=1:nbRepros
+ plot3(r(n).rxh(1,:), r(n).rxh(2,:), ones(1,nbData)*2, 'r-','linewidth',2);
+ plot3(r(n).lxh(1,:), r(n).lxh(2,:), ones(1,nbData)*2, 'r-','linewidth',2);
+end
+set(gca,'xtick',[],'ytick',[]); axis equal;
+
+%print('-dpng','graphs/demoIK_nullspace_TPGMM01.png');
+%pause;
+%close all;
+
+
diff --git a/demoIK_pointing_nullspace_TPGMM01.m b/demoIK_pointing_nullspace_TPGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..720e38ba718f7e2f1ddbb65bbf5ff98c00d46681
--- /dev/null
+++ b/demoIK_pointing_nullspace_TPGMM01.m
@@ -0,0 +1,323 @@
+function demoIK_pointing_nullspace_TPGMM01
+%Example of task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames)
+%(example with two objects and robot frame, starting from the same initial pose (nullspace constraint),
+%by using a single Euler orientation angle and 3 DOFs robot)
+%This example requires 'startup_rvc' to be run from the Robotics Toolbox.
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 6; %Number of states in the GMM
+model.nbFrames = 4; %Three candidate frames are defined: 1 in joint space and 2 in task space (2 objects)
+model.nbVars = [4,2,2,2]; %[[t,q],[t,e1],[t,e2]], where q are joint angles and e1,e2 are orientation offsets
+model.nbVar = model.nbVars(1); %Dimension for the product of Gaussians
+model.nbQ = model.nbVars(1)-1; %Number of articulations of the robot
+model.nbObj = 2; %Number of objects in the workspace
+model.dt = 0.01; %Time step
+nbSamples = 6; %Number of demonstrations
+nbRepros = 6; %Number of reproduction attempts
+nbData = 300; %Length of each trajectory
+nbDataRepro = nbData;
+eMax = 1; %Maximum error norm for stable Jacobian computation
+Kp = 0.15; %Amplification gain for error computation
+KpQ = 0.15; %Amplification gain for joint angle error computation
+
+needsData = 0;
+needsModel = 0;
+needsRepro = 0;
+
+%% Create robot (requires the Robotics Toolbox)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+armLength = 0.2; %length of each segment
+for i=1:model.nbQ
+ Lrob(i) = Link('d', 0, 'a', armLength, 'alpha', 0);
+end
+robot = SerialLink(Lrob);
+
+
+%% Demonstrations
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+if needsData==1
+disp('Generate data...');
+
+q0 = [0; pi/2; zeros(model.nbQ-2,1)];
+
+for n=1:nbSamples
+
+ %Set initial pose
+ s(n).q(:,1) = q0;
+
+ %Set object 1 position
+ oTmp = rand(2,1) .* [armLength*2; 2*pi/3] + [armLength*model.nbQ; pi/6];
+ s(n).obj(:,1,:) = repmat([oTmp(1)*cos(oTmp(2)); oTmp(1)*sin(oTmp(2))], 1, nbData); %rand(2,1).*[3;.5]+[-1;.6]
+
+ %Set object 2 position
+ oTmp = rand(2,1) .* [armLength*2; 2*pi/3] + [armLength*model.nbQ; pi/6];
+ s(n).obj(:,2,:) = repmat([oTmp(1)*cos(oTmp(2)); oTmp(1)*sin(oTmp(2))], 1, nbData);
+
+ %Motion loop
+ for t=1:nbData
+ %Computation of error terms for the two objects
+ Htmp = robot.fkine(s(n).q(:,t));
+ Etmp = tr2eul(Htmp);
+ s(n).x(:,t) = Etmp(3);
+ for j=1:model.nbObj
+ dir = s(n).obj(:,j,t) - Htmp(1:2,end);
+ xh = atan2(dir(2),dir(1));
+ e = xh - s(n).x(:,t);
+ if norm(e)>eMax
+ e = eMax * e / norm(e);
+ end
+ s(n).e(:,j,t) = e;
+ end
+
+ %Update of robot pose (through Jacobian)
+ Jtmp = robot.jacob0(s(n).q(:,t),'rot');
+ s(n).J(:,:,t) = Jtmp(3,:);
+ J = s(n).J(:,:,t);
+ pinvJ = pinv(J);
+ %pinvJ = (J'*J + eye(model.nbQ)*1E-8) \ J'; %Damped pseudoinverse
+ %W = diag([1,1,1]);
+ %pinvJ = (J'*W*J + eye(model.nbQ)*1E-8) \ J'*W; %Damped weighted pseudoinverse
+
+ if t<nbData/3
+ s(n).dq(:,t) = pinvJ * Kp * s(n).e(:,1,t)/model.dt;
+ %s(n).dq(2,t) = s(n).dq(2,t) * 2E-1; %Simulate weak articulation
+ elseif t<2*nbData/3
+ s(n).dq(:,t) = pinvJ * Kp * s(n).e(:,2,t)/model.dt;
+ %s(n).dq(2,t) = s(n).dq(2,t) * 2E-1; %Simulate weak articulation
+ else
+ %s(n).dq(:,t) = zeros(model.nbQ,1);
+ s(n).dq(:,t) = KpQ * (q0 - s(n).q(:,t))/model.dt;
+ end
+
+ %Nullspace control
+ N = eye(model.nbQ) - pinvJ*J;
+ s(n).dq(:,t) = s(n).dq(:,t) + N * KpQ * (q0 - s(n).q(:,t))/model.dt;
+ s(n).q(:,t+1) = s(n).q(:,t) + s(n).dq(:,t) * model.dt;
+ end
+end
+
+%Generate dataset
+Data = zeros(model.nbVar, model.nbFrames, nbData*nbSamples);
+for n=1:nbSamples
+ for t=1:nbData
+ Data(1:model.nbVars(1),1,(n-1)*nbData+t) = [t*model.dt; s(n).q(:,t+1) + randn(model.nbVars(1)-1,1)*1E-6];
+ Data(1:model.nbVars(2),2,(n-1)*nbData+t) = [t*model.dt; s(n).e(:,1,t) + randn(model.nbVars(2)-1,1)*1E-6];
+ Data(1:model.nbVars(3),3,(n-1)*nbData+t) = [t*model.dt; s(n).e(:,2,t) + randn(model.nbVars(3)-1,1)*1E-6];
+ Data(1:model.nbVars(4),4,(n-1)*nbData+t) = [t*model.dt; s(n).x(:,t) + randn(model.nbVars(4)-1,1)*1E-6];
+ end
+end
+
+save('data/TPGMMpointing_nullspace_data02.mat','s','Data');
+end %needsData
+load('data/TPGMMpointing_nullspace_data02.mat');
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+if needsModel==1
+fprintf('Parameters estimation of TP-GMM with EM:');
+model = init_TPGMM_timeBased(Data, model); %Initialization
+%model = init_TPGMM_kmeans(Data, model); %Initialization
+model = EM_TPGMM(Data, model);
+
+save('data/TPGMMpointing_nullspace_model02.mat','model');
+end %needsModel
+load('data/TPGMMpointing_nullspace_model02.mat');
+
+
+%% Reproduction with GMR
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+if needsRepro==1
+disp('Reproduction with GMR...');
+rr.Priors = model.Priors;
+rr.nbStates = model.nbStates;
+
+for n=1:nbRepros
+
+ %Set object 1 position
+ oTmp = rand(2,1) .* [armLength*2; 2*pi/3] + [armLength*model.nbQ; pi/6];
+ r(n).obj(:,1,:) = repmat([oTmp(1)*cos(oTmp(2)); oTmp(1)*sin(oTmp(2))], 1, nbDataRepro);
+
+ %Set object 2 position
+ oTmp = rand(2,1) .* [armLength*2; 2*pi/3] + [armLength*model.nbQ; pi/6];
+ r(n).obj(:,2,:) = repmat([oTmp(1)*cos(oTmp(2)); oTmp(1)*sin(oTmp(2))], 1, nbDataRepro);
+
+ %Initial pose of robot
+ %r(n).q(:,1) = rand(model.nbQ,1)*pi/4;
+ r(n).q(:,1) = s(n).q(:,1);
+
+ %Retrieval of motion
+ for t=1:nbDataRepro
+
+ %Compute relative orientation error
+ J = robot.jacob0(r(n).q(:,t),'rot');
+ J = J(3,:);
+ pinvJ = pinv(J);
+ Htmp = robot.fkine(r(n).q(:,t));
+ Etmp = tr2eul(Htmp);
+ r(n).x(:,t) = Etmp(3);
+ for j=1:model.nbObj
+ dir = r(n).obj(:,j,t) - Htmp(1:2,end);
+ xh = atan2(dir(2),dir(1));
+ e(:,j) = xh - r(n).x(:,t);
+ if norm(e(:,j))>eMax
+ e(:,j) = eMax * e(:,j) / norm(e(:,j));
+ end
+ r(n).e(:,j,t) = e(:,j);
+ end
+
+ %Update Frame 1 (null space)
+ N = eye(model.nbQ) - pinvJ*J;
+ pTmp(1).A = [1 zeros(1,model.nbVars(1)-1); zeros(model.nbQ,1) N*KpQ];
+ pTmp(1).b = [0; r(n).q(:,t)-N*KpQ*r(n).q(:,t)];
+ %pTmp(1).b = [0; pinv(Jtmp)*Jtmp*r(n).q(:,t)]; %Correct only for KpQ=1
+
+ %Update Frame 2 (task space)
+ pTmp(2).A = [1 zeros(1,model.nbVars(2)-1); zeros(model.nbQ,1) pinvJ*Kp];
+ pTmp(2).b = [0; r(n).q(:,t)+pinvJ*Kp*r(n).e(:,1,t)];
+
+ %Update Frame 3 (task space)
+ pTmp(3).A = [1 zeros(1,model.nbVars(3)-1); zeros(model.nbQ,1) pinvJ*Kp];
+ pTmp(3).b = [0; r(n).q(:,t)+pinvJ*Kp*r(n).e(:,2,t)];
+
+ %Update Frame 4 (task space)
+ pTmp(4).A = [1 zeros(1,model.nbVars(4)-1); zeros(model.nbQ,1) pinvJ];
+ pTmp(4).b = [0; r(n).q(:,t)-pinvJ*r(n).x(:,t)];
+
+ %TP-GMR
+ [rr.Mu, rr.Sigma] = productTPGMM(model, pTmp);
+ r(n).q(:,t+1) = GMR(rr, t*model.dt, 1, 2:model.nbVars(1));
+ end
+end
+
+save('data/TPGMMpointing_nullspace_repro02.mat','r');
+end %needsRepro
+load('data/TPGMMpointing_nullspace_repro02.mat');
+
+
+%% Plot timelines
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,600,680]);
+fTmp = [1 3 5; 2 0 0; 4 0 0; 6 0 0];
+for m=1:model.nbFrames
+ for k=1:model.nbVars(m)-1
+ subplot(model.nbVar-1, 2, fTmp(m,k)); hold on;
+ plotGMM(squeeze(model.Mu([1,k+1],m,:)), squeeze(model.Sigma([1,k+1],[1,k+1],m,:))+repmat(eye(2)*1E-4,[1 1 model.nbStates]), [0 .7 0]);
+ for n=1:nbSamples
+ plot(squeeze(Data(1,m,(n-1)*nbData+1:n*nbData)), squeeze(Data(k+1,m,(n-1)*nbData+1:n*nbData)), '-','color',[.3 .3 .3]);
+ end
+ if m==1
+ for n=1:nbRepros
+ plot(squeeze(Data(1,m,1:nbData)), r(n).q(k,1:nbData), '-','color',[.8 0 0],'linewidth',1.5);
+ end
+ elseif m==2 || m==3
+ for n=1:nbRepros
+ plot(squeeze(Data(1,m,1:nbData)), squeeze(r(n).e(k,m-1,:)), '-','color',[.8 0 0],'linewidth',1.5);
+ end
+ else
+ for n=1:nbRepros
+ plot(squeeze(Data(1,m,1:nbData)), squeeze(r(n).x(1,:)), '-','color',[.8 0 0],'linewidth',1.5);
+ end
+ end
+ xlabel('$t$','interpreter','latex','fontsize',14);
+ set(gca,'xtick', [model.dt, nbData*model.dt], 'xticklabel',{'0','1'});
+ ylabel(['$X^{(' num2str(m) ')}_' num2str(k) '$'],'interpreter','latex','fontsize',16);
+ if k==1
+ if m==1
+ title('Frame 1 (preferred pose)','fontsize',10);
+ elseif m==2
+ title('Frame 2 (red object)','fontsize',10);
+ elseif m==3
+ title('Frame 3 (blue object)','fontsize',10);
+ elseif m==4
+ title('Frame 4 (robot frame)','fontsize',10);
+ end
+ end
+ end
+end
+
+
+% %% Plots 2D anim
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10,10,1200,600],'color',[1,1,1]);
+% hold on; axis off; axis equal;
+% h=[];
+% for n=1:nbRepros
+% for t=round(linspace(1,nbDataRepro,nbDataRepro/4))
+% delete(h);
+% h = plotArm(r(n).q(:,t), [ones(model.nbQ-1,1)*armLength; armLength*5], [0; 0; -n*2+(t/nbDataRepro)], .002, [1 .7 .7],[1 .7 .7]);
+% h = [h, plotArm(r(n).q(:,t), ones(model.nbQ,1)*armLength, [0; 0; 0], .05, [.7 .7 1])];
+% h = [h, plot(r(n).obj(1,1,t), r(n).obj(2,1,t), '.','markersize',20,'color',[.8 0 0])];
+% h = [h, plot(r(n).obj(1,2,t), r(n).obj(2,2,t), '.','markersize',20,'color',[0 0 .8])];
+% axis([-1 2 -.2 1.1]);
+% %pause(0.02);
+% drawnow;
+% if t<3
+% pause;
+% end
+% end
+% end
+
+
+%% Plots 2D demos static
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+tList = [1, nbData/6, nbData/3+nbData/6, nbData];
+figure('PaperPosition',[0 0 12 4.5],'position',[10,10,1200,450],'color',[1,1,1]);
+set(0,'DefaultAxesLooseInset',[0,0,0,0]);
+for n=1:nbSamples
+ for j=2:length(tList)
+ subplot(length(tList)-1,nbSamples, (j-2)*nbSamples+n); hold on; axis off;
+ if j==2
+ title(['Demonstration ' num2str(n)],'fontsize',10);
+ end
+ plotArm(s(n).q(:,tList(j)), [ones(model.nbQ-1,1)*armLength; armLength*4], [0; 0; -1], .002, [.7 .7 .7],[.7 .7 .7]);
+ ql = ones(model.nbQ,1) * 999;
+ for t=tList(j-1):tList(j)
+ if norm(ql-s(n).q(:,t))>0.08 || t==tList(j)
+ colTmp = [.3 .3 .3] + [.5 .5 .5] * (tList(j)-t)/(tList(j)-tList(j-1));
+ plotArm(s(n).q(:,t), ones(model.nbQ,1)*armLength, [0; 0; t/2], .05, colTmp);
+ plot(s(n).obj(1,1,t), s(n).obj(2,1,t), '.','markersize',20,'color',[.8 0 0]);
+ plot(s(n).obj(1,2,t), s(n).obj(2,2,t), '.','markersize',20,'color',[0 0 .8]);
+ ql = s(n).q(:,t);
+ end
+ end
+ text(0.1, -0.2, ['t=' num2str(tList(j)/100,'%.1f')]);
+ axis equal; axis([-1 1 -.2 1.1]);
+ end
+end
+
+
+%% Plots 2D repros static
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('PaperPosition',[0 0 12 4.5],'position',[10,10,1200,450],'color',[1,1,1]);
+set(0,'DefaultAxesLooseInset',[0,0,0,0]);
+for n=1:nbRepros
+ for j=2:length(tList)
+ subplot(length(tList)-1,nbRepros, (j-2)*nbRepros+n); hold on; axis off;
+ if j==2
+ title(['Reproduction ' num2str(n)],'fontsize',10);
+ end
+ plotArm(r(n).q(:,tList(j)), [ones(model.nbQ-1,1)*armLength; armLength*4], [0; 0; -1], .002, [.7 .7 .7],[.7 .7 .7]);
+ ql = ones(model.nbQ,1) * 999;
+ for t=tList(j-1):tList(j)
+ if norm(ql-r(n).q(:,t))>0.08 || t==tList(j)
+ colTmp = [.3 .3 .7] + [.5 .5 .3] * (tList(j)-t)/(tList(j)-tList(j-1));
+ plotArm(r(n).q(:,t), ones(model.nbQ,1)*armLength, [0; 0; t/2], .05, colTmp);
+ plot(r(n).obj(1,1,t), r(n).obj(2,1,t), '.','markersize',20,'color',[.8 0 0]);
+ plot(r(n).obj(1,2,t), r(n).obj(2,2,t), '.','markersize',20,'color',[0 0 .8]);
+ ql = r(n).q(:,t);
+ end
+ end
+ text(0.1, -0.2, ['t=' num2str(tList(j)/100,'%.1f')]);
+ axis equal; axis([-1 1 -.2 1.1]);
+ end
+end
+
+%print('-dpng','graphs/demoIK_pointing_nullspace_TPGMM01.png');
+%pause;
+%close all;
+
diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..350709f648a681966e174722e43adcf3264a1663
--- /dev/null
+++ b/demo_DSGMR01.m
@@ -0,0 +1,74 @@
+function demo_DSGMR01
+%Example of Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction,
+%with decay variable used as input (as in DMP)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 5; %Number of states in the GMM
+model.nbVar = 3; %Number of variables [t,x1,x2]
+nbData = 200; %Length of each trajectory
+model.dt = 0.001; %Time step
+nbSamples = 5; %Number of demonstrations
+
+model.kP = 2000; %Stiffness gain
+model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
+model.alpha = 1.0; %Decay factor
+nbVarOut = model.nbVar-1;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+
+%% Load AMARSI data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+sIn(1) = 1; %Initialization of decay term
+for t=2:nbData
+ sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
+end
+
+%Store data as [s; x]
+demos=[];
+load('data/AMARSI/CShape.mat');
+Data=[];
+for n=1:nbSamples
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+ Data = [Data [sIn; s(n).Data]];
+end
+
+
+%% Learning and reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%model = init_GMM_timeBased(Data, model);
+model = init_GMM_kmeans(Data, model);
+model = EM_GMM(Data, model);
+[currTar, currSigma] = GMR(model, sIn, 1, 2:3);
+%Motion retrieval with spring-damper system
+x = Data(2:model.nbVar,1);
+dx = zeros(nbVarOut,1);
+for t=1:nbData
+ %Compute acceleration, velocity and position
+ ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+ dx = dx + ddx * model.dt;
+ x = x + dx * model.dt;
+ DataOut(:,t) = x;
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]);
+%Plot GMM
+subplot(1,2,1); hold on; box on; title('GMM');
+plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0]);
+plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+%Plot DS-GMR
+subplot(1,2,2); hold on; box on; title('DS-GMR');
+plotGMM(currTar, currSigma, [0 .8 0]);
+plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
+plot(currTar(1,:),currTar(2,:),'-','linewidth',1.5,'color',[0 .6 0]);
+plot(DataOut(1,:),DataOut(2,:),'-','linewidth',3,'color',[0 .3 0]);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+
+%print('-dpng','graphs/demo_DSGMR01.png');
+%pause;
+%close all;
diff --git a/demo_GMM01.m b/demo_GMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..11aade306899848221683732fbcde5e69a0f0145
--- /dev/null
+++ b/demo_GMM01.m
@@ -0,0 +1,41 @@
+function demo_GMM01
+%Example of Gaussian mixture model (GMM)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 5; %Number of states in the GMM
+model.nbVar = 2; %Number of variables [x1,x2]
+nbData = 200; %Length of each trajectory
+
+
+%% Load AMARSI data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/AMARSI/GShape.mat');
+nbSamples = length(demos);
+Data=[];
+for n=1:nbSamples
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+ Data = [Data s(n).Data];
+end
+
+
+%% Parameters estimation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model = init_GMM_kmeans(Data, model);
+model = EM_GMM(Data, model);
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]); hold on; box on;
+plotGMM(model.Mu, model.Sigma, [.8 0 0]);
+plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+
+%print('-dpng','graphs/demo_GMM01.png');
+%pause;
+%close all;
diff --git a/demo_GMR01.m b/demo_GMR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..afe5c267e73d0b5313110b05183918afd7bdb9e9
--- /dev/null
+++ b/demo_GMR01.m
@@ -0,0 +1,51 @@
+function demo_GMR01
+%Example of Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 5; %Number of states in the GMM
+model.nbVar = 3; %Number of variables [t,x1,x2]
+model.dt = 0.001; %Time step duration
+nbData = 200; %Length of each trajectory
+nbSamples = 5; %Number of demonstrations
+
+%% Load AMARSI data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/AMARSI/CShape.mat');
+Data=[];
+for n=1:nbSamples
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+ Data = [Data [[1:nbData]*model.dt; s(n).Data]];
+end
+
+
+%% Learning and reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%model = init_GMM_kmeans(Data, model);
+model = init_GMM_timeBased(Data, model);
+model = EM_GMM(Data, model);
+[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:3);
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]);
+%Plot GMM
+subplot(1,2,1); hold on; box on; title('GMM');
+plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0]);
+plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+%Plot GMR
+subplot(1,2,2); hold on; box on; title('GMR');
+plotGMM(DataOut, SigmaOut, [0 .8 0]);
+plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
+plot(DataOut(1,:),DataOut(2,:),'-','linewidth',2,'color',[0 .4 0]);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+
+%print('-dpng','graphs/demo_GMR01.png');
+%pause;
+%close all;
diff --git a/demo_GPR01.m b/demo_GPR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..67670111af308af19bed0141ab4689233889a108
--- /dev/null
+++ b/demo_GPR01.m
@@ -0,0 +1,230 @@
+function demo_GPR01
+%Example of Gaussian processs regression (GPR) used as a task-parameterized model,
+%with DS-GMR employed to retrieve continuous movements
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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 (for DS-GMR)
+model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load motion 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
+% 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/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 data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+Data = s(1).Data0(1,:);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D];
+ Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
+end
+
+
+%% Prepare input-output data for GPR from GMM encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of GPR with GMM encoding:');
+%GMM encoding for each trajectory sample, learned at once by stacking the different trajectory samples in 'Data' matrix of size M*D x T
+model.nbVar = size(Data,1); %Temporary modification of nbVar (for stacked data training)
+model = init_GMM_timeBased(Data, model);
+model = EM_GMM(Data, model);
+model.nbVar = size(s(1).p(1).A,1); %Setting back the initial nbVar
+for n=1:nbSamples
+ id = (n-1)*2+2:n*2+1;
+ s(n).Priors = model.Priors;
+ s(n).Mu = model.Mu([1,id],:);
+ s(n).Sigma = model.Sigma([1,id],[1,id],:);
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(s(n).Sigma(:,:,i));
+ s(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
+ end
+ %Set query point vector (position and orientation of the two objects)
+ s(n).DataIn = [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)];
+ model.DataIn(:,n) = s(n).DataIn;
+ %Set model output vector (Mu and Sigma)
+ model.DataOut(:,n) = [reshape(s(n).Mu, model.nbVar*model.nbStates, 1); reshape(s(n).Sigma, model.nbVar^2*model.nbStates, 1)];
+end
+
+
+%% Reproduction with GPR 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;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+for n=1:nbSamples
+ %Rebuild model parameters with GPR
+ vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn);
+
+ %Re-arrange GPR output as GMM parameters
+ r(n).Mu = reshape(vOut(1:model.nbVar*model.nbStates), model.nbVar, model.nbStates);
+ r(n).Sigma = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+ r(n).Priors = model.Priors;
+ r(n).nbStates = model.nbStates;
+
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(r(n).Sigma(:,:,i));
+ r(n).Sigma(:,:,i) = V * max(D,5E-4) * V';
+ end
+
+ %Retrieval of attractor path through GMR
+ currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+
+ %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
+ ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+ dx = dx + ddx * model.dt;
+ x = x + dx * model.dt;
+ r(n).Data(:,t) = x;
+ end
+end
+
+
+%% Reproduction with GPR and DS-GMR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with DS-GMR...');
+for n=1:nbRepros
+ %Random generation of new task parameters
+ for m=1:model.nbFrames
+ 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
+ %Query point vector (position and orientation of the two objects)
+ rnew(n).DataIn = [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)];
+
+ %Rebuild model parameters with GPR
+ vOut = GPR(model.DataIn, model.DataOut, rnew(n).DataIn);
+
+ %Re-arrange GPR output as GMM parameters
+ rnew(n).Mu = reshape(vOut(1:model.nbVar*model.nbStates), model.nbVar, model.nbStates);
+ rnew(n).Sigma = reshape(vOut(model.nbVar*model.nbStates+1:end), model.nbVar, model.nbVar, model.nbStates);
+ rnew(n).Priors = model.Priors;
+ rnew(n).nbStates = model.nbStates;
+
+ %Regularization of Sigma
+ for i=1:model.nbStates
+ [V,D] = eig(rnew(n).Sigma(:,:,i));
+ rnew(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
+ end
+
+ %Retrieval of attractor path through GMR
+ currTar = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+
+ %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-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+ dx = dx + ddx * model.dt;
+ x = x + dx * model.dt;
+ rnew(n).Data(:,t) = x;
+ 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(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,:));
+end
+for n=1:nbSamples
+ %Plot Gaussians
+ plotGMM(s(n).Mu(2:3,:), s(n).Sigma(2:3,2:3,:), [.5 .5 .5],.6);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with GPR');
+for n=1:nbSamples
+ %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
+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
+for n=1:nbSamples
+ %Plot Gaussians
+ plotGMM(r(n).Mu(2:3,:), r(n).Sigma(2:3,2:3,:), [.5 .5 .5],.6);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with GPR');
+for n=1:nbRepros
+ %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
+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
+for n=1:nbRepros
+ %Plot Gaussians
+ plotGMM(rnew(n).Mu(2:3,:), rnew(n).Sigma(2:3,2:3,:), [.5 .5 .5],.6);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_GPR01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_HDDC01.m b/demo_HDDC01.m
new file mode 100644
index 0000000000000000000000000000000000000000..358afbe14106f8ac957d4f9a03c33c67a09d65c8
--- /dev/null
+++ b/demo_HDDC01.m
@@ -0,0 +1,52 @@
+function demo_HDDC01
+%Example of High Dimensional Data Clustering (HDDC, or HD-GMM) model from Bouveyron (2007)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 4; %Number of states in the GMM
+model.nbVar = 4; %Number of variables [x1,x2,x3,x4]
+model.nbFA = 1; %Dimension of the subspace
+nbData = 200; %Length of each trajectory
+nbSamples = 5; %Number of demonstrations
+
+
+%% Load AMARSI data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
+for n=1:nbSamples
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+end
+demos=[];
+load('data/AMARSI/CShape.mat'); %Load x3,x4 variables
+Data=[];
+for n=1:nbSamples
+ s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
+ Data = [Data s(n).Data];
+end
+
+
+%% Parameters estimation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model = init_GMM_kmeans(Data, model);
+model0 = EM_GMM(Data, model); %for comparison
+model = EM_HDGMM(Data, model);
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]);
+for i=1:2
+ subplot(1,2,i); hold on; box on;
+ plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8]);
+ plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0]);
+ plot(Data((i-1)*2+1,:),Data(i*2,:),'.','markersize',8,'color',[.7 .7 .7]);
+ axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+ xlabel(['x_' num2str((i-1)*2+1)]); ylabel(['x_' num2str(i*2)]);
+end
+
+%print('-dpng','graphs/demo_HDDC01.png');
+%pause;
+%close all;
diff --git a/demo_MFA01.m b/demo_MFA01.m
new file mode 100644
index 0000000000000000000000000000000000000000..f1f575c9cf6cc4f1965b988062132b49b331afd9
--- /dev/null
+++ b/demo_MFA01.m
@@ -0,0 +1,53 @@
+function demo_MFA01
+%Example of mixture of factor analysers (MFA)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 4; %Number of states in the GMM
+model.nbVar = 4; %Number of variables [x1,x2,x3,x4]
+model.nbFA = 1; %Dimension of the subspace (number of factor analyzers)
+nbData = 200; %Length of each trajectory
+nbSamples = 5; %Number of demonstrations
+
+
+%% Load AMARSI data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
+for n=1:nbSamples
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+end
+demos=[];
+load('data/AMARSI/CShape.mat'); %Load x3,x4 variables
+Data=[];
+for n=1:nbSamples
+ s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
+ Data = [Data s(n).Data];
+end
+
+
+%% Parameters estimation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model = init_GMM_kmeans(Data, model);
+model0 = EM_GMM(Data, model); %for comparison
+model = EM_MFA(Data, model);
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]);
+for i=1:2
+ subplot(1,2,i); hold on; box on;
+ plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8]);
+ plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0]);
+ plot(Data((i-1)*2+1,:),Data(i*2,:),'.','markersize',8,'color',[.7 .7 .7]);
+ axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+ xlabel(['x_' num2str((i-1)*2+1)]); ylabel(['x_' num2str(i*2)]);
+end
+
+%print('-dpng','graphs/demo_MFA01.png');
+%pause;
+%close all;
diff --git a/demo_MPPCA01.m b/demo_MPPCA01.m
new file mode 100644
index 0000000000000000000000000000000000000000..973ea06fd193fb9ea03f9ce1efbd6b875000eb28
--- /dev/null
+++ b/demo_MPPCA01.m
@@ -0,0 +1,53 @@
+function demo_MPPCA01
+%Example of mixture of probabilistic principal component analyzers (MPPCA)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 4; %Number of states in the GMM
+model.nbVar = 4; %Number of variables [x1,x2,x3,x4]
+model.nbFA = 1; %Dimension of the subspace (number of principal components)
+nbData = 200; %Length of each trajectory
+nbSamples = 5; %Number of demonstrations
+
+
+%% Load AMARSI data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
+for n=1:nbSamples
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+end
+demos=[];
+load('data/AMARSI/CShape.mat'); %Load x3,x4 variables
+Data=[];
+for n=1:nbSamples
+ s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
+ Data = [Data s(n).Data];
+end
+
+
+%% Parameters estimation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model = init_GMM_kmeans(Data, model);
+model0 = EM_GMM(Data, model); %for comparison
+model = EM_MPPCA(Data, model);
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]);
+for i=1:2
+ subplot(1,2,i); hold on; box on;
+ plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8]);
+ plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0]);
+ plot(Data((i-1)*2+1,:),Data(i*2,:),'.','markersize',8,'color',[.7 .7 .7]);
+ axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+ xlabel(['x_' num2str((i-1)*2+1)]); ylabel(['x_' num2str(i*2)]);
+end
+
+%print('-dpng','graphs/demo_MPPCA01.png');
+%pause;
+%close all;
diff --git a/demo_TPGMM01.m b/demo_TPGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..c92750c904c73f984f23ba0e2d2c4dfb4bbcdf09
--- /dev/null
+++ b/demo_TPGMM01.m
@@ -0,0 +1,79 @@
+function demo_TPGMM01
+%Example of task-parameterized Gaussian mixture model (TP-GMM)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2)
+
+
+%% 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');
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-GMM with EM:');
+model = init_tensorGMM_kmeans(Data, model);
+model = EM_tensorGMM(Data, model);
+
+%Reconstruct GMM for each demonstration
+for n=1:nbSamples
+ [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+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,model.nbFrames+1,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).Data(2,1), s(n).Data(3,1),'.','markersize',15,'color',clrmap(n,:));
+ plot(s(n).Data(2,:), s(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+ %Plot Gaussians
+ plotGMM(s(n).Mu, s(n).Sigma, [.5 .5 .5],.8);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%FRAMES
+for m=1:model.nbFrames
+ subplot(1,model.nbFrames+1,1+m); hold on; grid on; box on; title(['Frame ' num2str(m)]);
+ for n=1:nbSamples
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
+ end
+ plotGMM(squeeze(model.Mu(:,m,:)), squeeze(model.Sigma(:,:,m,:)), [.5 .5 .5],.8);
+ axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%print('-dpng','graphs/demo_TPGMM01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_TPGMR01.m b/demo_TPGMR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..b0e10214e32404ab4506aff157ae639e32a2d26c
--- /dev/null
+++ b/demo_TPGMR01.m
@@ -0,0 +1,133 @@
+function demo_TPGMR01
+%Example of task-parameterized Gaussian mixture model (TP-GMM), with GMR used for reproduction
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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)
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+
+
+%% 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
+% 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/Data02.mat');
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-GMM with EM:');
+%model = init_tensorGMM_kmeans(Data, model);
+model = init_tensorGMM_timeBased(Data, model);
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with GMR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with GMR...');
+DataIn = s(1).Data(1,:);
+for n=1:nbSamples
+ [r(n).Mu, r(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
+ r(n).Priors = model.Priors;
+ r(n).nbStates = model.nbStates;
+ r(n).Data = [DataIn; GMR(r(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+end
+
+
+%% Reproduction with GMR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with GMR...');
+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
+ %Retrieval of attractor path through task-parameterized GMR
+ [rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
+ rnew(n).Priors = model.Priors;
+ rnew(n).nbStates = model.nbStates;
+ rnew(n).Data = [DataIn; GMR(rnew(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+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(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).Data(2,1), s(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+ plot(s(n).Data(2,:), s(n).Data(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 with GMR');
+for n=1:nbSamples
+ %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
+end
+for n=1:nbSamples
+ %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,:));
+end
+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
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with GMR');
+for n=1:nbRepros
+ %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
+end
+for n=1:nbRepros
+ %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]);
+end
+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
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_TPGMR01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_TPGMR_DS01.m b/demo_TPGMR_DS01.m
index 563b65ca0d48cbae609d97ff18e728926d35c6d5..1d6a20fac2b99bbee1bea22f92697582ac55e2bf 100644
--- a/demo_TPGMR_DS01.m
+++ b/demo_TPGMR_DS01.m
@@ -67,9 +67,9 @@ for n=1:nbSamples
end
-%% Tensor GMM learning
+%% TP-GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of tensor GMM with EM:');
+fprintf('Parameters estimation of TP-GMM with EM:');
model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model);
@@ -133,14 +133,16 @@ for n=1:nbSamples
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 Gaussians
- plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbSamples
%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,:));
end
+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
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%NEW REPROS
@@ -151,17 +153,20 @@ for n=1:nbRepros
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
- %Plot Gaussians
- plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbRepros
%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]);
end
+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
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-%print('-dpng','outTest1.png');
+%print('-dpng','graphs/demo_TPGMR_DS01.png');
+
%% Plot additional information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -179,7 +184,6 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('|Kp|');
-
%pause;
%close all;
diff --git a/demo_TPGMR_LQR01.m b/demo_TPGMR_LQR01.m
index 63a204c693756357e1e3dc1f2bc236d80d1249ab..0ad618841e51dabb83c83b0c856f13ed7f580514 100644
--- a/demo_TPGMR_LQR01.m
+++ b/demo_TPGMR_LQR01.m
@@ -34,7 +34,7 @@ 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
nbRepros = 8; %Number of reproductions with new situations randomly generated
-rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
%% Load 3rd order tensor data
@@ -49,9 +49,9 @@ disp('Load 3rd order tensor data...');
load('data/DataLQR01.mat');
-%% Tensor GMM learning
+%% TP-GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of tensor GMM with EM:');
+fprintf('Parameters estimation of TP-GMM with EM:');
model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model);
@@ -108,42 +108,50 @@ end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%REPROS
-subplot(1,3,2); hold on; box on; title('Reproductions with finite horizon LQR');
+subplot(1,3,2); hold on; box on; title('Repros with finite horizon LQR');
for n=1:nbSamples
%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 Gaussians
- plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbSamples
%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,:));
end
+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
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%NEW REPROS
-subplot(1,3,3); hold on; box on; title('New reproductions with finite horizon LQR');
+subplot(1,3,3); hold on; box on; title('New repros with finite horizon LQR');
for n=1:nbRepros
%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
- %Plot Gaussians
- plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbRepros
%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]);
end
+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
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-%Plot additional information
+%print('-dpng','graphs/demo_TPGMR_LQR01.png');
+
+
+%% Plot additional information
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
%Plot norm of control commands
subplot(1,2,1); hold on;
@@ -158,7 +166,6 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('|Kp|');
-
% %Plot accelerations due to feedback and feedforward terms
% figure; hold on;
% n=1; k=1;
@@ -167,6 +174,5 @@ xlabel('t'); ylabel('|Kp|');
% legend('ddx feedback','ddx feedforward');
% xlabel('t'); ylabel(['ddx_' num2str(k)]);
-
%pause;
%close all;
diff --git a/demo_TPGMR_LQR02.m b/demo_TPGMR_LQR02.m
index 50ca8011498b4e1dd6b9a7f9a30abcf363d9b9b6..4f733c716555361efc553179ba89cd753295f973 100644
--- a/demo_TPGMR_LQR02.m
+++ b/demo_TPGMR_LQR02.m
@@ -34,7 +34,7 @@ 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
nbRepros = 8; %Number of reproductions with new situations randomly generated
-rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
%% Load 3rd order tensor data
@@ -49,9 +49,9 @@ disp('Load 3rd order tensor data...');
load('data/DataLQR01.mat');
-%% Tensor GMM learning
+%% TP-GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of tensor GMM with EM:');
+fprintf('Parameters estimation of TP-GMM with EM:');
model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model);
@@ -108,42 +108,50 @@ end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%REPROS
-subplot(1,3,2); hold on; box on; title('Reproductions with infinite horizon LQR');
+subplot(1,3,2); hold on; box on; title('Repros with infinite horizon LQR');
for n=1:nbSamples
%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 Gaussians
- plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbSamples
%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,:));
end
+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
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%NEW REPROS
-subplot(1,3,3); hold on; box on; title('New reproductions with infinite horizon LQR');
+subplot(1,3,3); hold on; box on; title('New repros with infinite horizon LQR');
for n=1:nbRepros
%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
- %Plot Gaussians
- plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbRepros
%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]);
end
+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
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-%Plot additional information
+%print('-dpng','graphs/demo_TPGMR_LQR02.png');
+
+
+%% Plot additional information
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
%Plot norm of control commands
subplot(1,2,1); hold on;
@@ -158,7 +166,6 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('|Kp|');
-
% %Plot accelerations due to feedback and feedforward terms
% figure; hold on;
% n=1; k=1;
@@ -167,6 +174,5 @@ xlabel('t'); ylabel('|Kp|');
% legend('ddx feedback','ddx feedforward');
% xlabel('t'); ylabel(['ddx_' num2str(k)]);
-
%pause;
%close all;
diff --git a/demo_TPHDDC01.m b/demo_TPHDDC01.m
new file mode 100644
index 0000000000000000000000000000000000000000..9dab570218fc6db9ea366af841e6a8c4e0f7eed5
--- /dev/null
+++ b/demo_TPHDDC01.m
@@ -0,0 +1,80 @@
+function demo_TPHDDC01
+%Example of task-parameterized high dimensional data clustering (TP-HDDC)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2)
+model.nbFA = 1; %Dimension of the subspace
+
+
+%% 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');
+
+
+%% TP-HDDC learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-HDDC with EM:');
+model = init_tensorGMM_kmeans(Data, model);
+model = EM_tensorHDGMM(Data, model);
+
+%Reconstruct GMM for each demonstration
+for n=1:nbSamples
+ [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+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,model.nbFrames+1,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).Data(2,1), s(n).Data(3,1),'.','markersize',15,'color',clrmap(n,:));
+ plot(s(n).Data(2,:), s(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+ %Plot Gaussians
+ plotGMM(s(n).Mu, s(n).Sigma, [.5 .5 .5],.8);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%FRAMES
+for m=1:model.nbFrames
+ subplot(1,model.nbFrames+1,1+m); hold on; grid on; box on; title(['Frame ' num2str(m)]);
+ for n=1:nbSamples
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
+ end
+ plotGMM(squeeze(model.Mu(:,m,:)), squeeze(model.Sigma(:,:,m,:)), [.5 .5 .5],.8);
+ axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%print('-dpng','graphs/demo_TPHDDC01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_TPMFA01.m b/demo_TPMFA01.m
new file mode 100644
index 0000000000000000000000000000000000000000..b47018794d21fa61572ae14dfc5d8fc7dda82f46
--- /dev/null
+++ b/demo_TPMFA01.m
@@ -0,0 +1,80 @@
+function demo_TPMFA01
+%Example of task-parameterized mixture of factor analyzers (TP-MFA)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2)
+model.nbFA = 1; %Dimension of the subspace (number of factor analyzers)
+
+
+%% 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');
+
+
+%% TP-MFA learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-MFA with EM:');
+model = init_tensorGMM_kmeans(Data, model);
+model = EM_tensorMFA(Data, model);
+
+%Reconstruct GMM for each demonstration
+for n=1:nbSamples
+ [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+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,model.nbFrames+1,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).Data(2,1), s(n).Data(3,1),'.','markersize',15,'color',clrmap(n,:));
+ plot(s(n).Data(2,:), s(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+ %Plot Gaussians
+ plotGMM(s(n).Mu, s(n).Sigma, [.5 .5 .5],.8);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%FRAMES
+for m=1:model.nbFrames
+ subplot(1,model.nbFrames+1,1+m); hold on; grid on; box on; title(['Frame ' num2str(m)]);
+ for n=1:nbSamples
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
+ end
+ plotGMM(squeeze(model.Mu(:,m,:)), squeeze(model.Sigma(:,:,m,:)), [.5 .5 .5],.8);
+ axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%print('-dpng','graphs/demo_TPMFA01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_TPMPPCA01.m b/demo_TPMPPCA01.m
new file mode 100644
index 0000000000000000000000000000000000000000..4a9071307554e9a61bd427c270743b1815444d73
--- /dev/null
+++ b/demo_TPMPPCA01.m
@@ -0,0 +1,80 @@
+function demo_TPMPPCA01
+%Example of task-parameterized mixture of probabilistic principal component analyzers (MPPCA) (TP-GMM)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2)
+model.nbFA = 1; %Dimension of the subspace (number of principal components)
+
+
+%% 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');
+
+
+%% TP-MPPCA learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-MPPCA with EM:');
+model = init_tensorGMM_kmeans(Data, model);
+model = EM_tensorMPPCA(Data, model);
+
+%Reconstruct GMM for each demonstration
+for n=1:nbSamples
+ [s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+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,model.nbFrames+1,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).Data(2,1), s(n).Data(3,1),'.','markersize',15,'color',clrmap(n,:));
+ plot(s(n).Data(2,:), s(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+ %Plot Gaussians
+ plotGMM(s(n).Mu, s(n).Sigma, [.5 .5 .5],.8);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%FRAMES
+for m=1:model.nbFrames
+ subplot(1,model.nbFrames+1,1+m); hold on; grid on; box on; title(['Frame ' num2str(m)]);
+ for n=1:nbSamples
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+ plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), ...
+ squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
+ end
+ plotGMM(squeeze(model.Mu(:,m,:)), squeeze(model.Sigma(:,:,m,:)), [.5 .5 .5],.8);
+ axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%print('-dpng','graphs/demo_TPMPPCA01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_stdPGMM01.m b/demo_stdPGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..675377300dc9e175c91aa770e2b5e4e91e732f90
--- /dev/null
+++ b/demo_stdPGMM01.m
@@ -0,0 +1,208 @@
+function demo_stdPGMM01
+%Example of parametric Gaussian mixture model (PGMM) used as a task-parameterized model,
+%with DS-GMR employed to retrieve continuous movements
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% 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 (for DS-GMR)
+model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load motion 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
+% 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/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 data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+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
+ %Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+ 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. (7.1.4) in doc/TechnicalReport.pdf
+% 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_timeBased(Data, model);
+model = EM_GMM(Data, model);
+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);
+ % %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;
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+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. (7.1.4) in doc/TechnicalReport.pdf
+ end
+ r(n).Mu = model.Mu;
+ %Retrieval of attractor path through GMR
+ currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+ %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
+ ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+ 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...');
+for n=1:nbRepros
+ %Random generation of new task parameters
+ for m=1:model.nbFrames
+ 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
+
+ %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+ 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
+% 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
+ end
+ rnew(n).Mu = model.Mu;
+ %Retrieval of attractor path through GMR
+ currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+ %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-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+ dx = dx + ddx * model.dt;
+ x = x + dx * model.dt;
+ rnew(n).Data(:,t) = x;
+ 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(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,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with PGMM');
+for n=1:nbSamples
+ %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
+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
+for n=1:nbSamples
+ %Plot Gaussians
+ plotGMM(r(n).Mu(2:3,:), model.Sigma(2:3,2:3,:), [.5 .5 .5],.6);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with PGMM');
+for n=1:nbRepros
+ %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
+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
+for n=1:nbRepros
+ %Plot Gaussians
+ plotGMM(rnew(n).Mu(2:3,:), model.Sigma(2:3,2:3,:), [.5 .5 .5],.6);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_stdPGMM01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_testLQR01.m b/demo_testLQR01.m
index edbe4bcc6cb08f0f0891de4eeccbf22d70b86726..08e0e0750d42e4e6203b7193ad5613903facb8e8 100644
--- a/demo_testLQR01.m
+++ b/demo_testLQR01.m
@@ -1,5 +1,5 @@
function demo_testLQR01
-% Test of the linear quadratic regulation
+% Test of the linear quadratic regulation with different variance in the data
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
diff --git a/demo_testLQR02.m b/demo_testLQR02.m
index 39bfaeafc16b7f0d0500b8c6f38a79ed0b678d55..787a1d7d72bf6cb8c22169dc4fee50a2a62407d7 100644
--- a/demo_testLQR02.m
+++ b/demo_testLQR02.m
@@ -70,8 +70,8 @@ for n=1:nbRepros
dampingRatio = r(n).kvDet(:) ./ (2*r(n).kpDet(:).^.5);
plot(DataIn, dampingRatio, 'k-', 'linewidth', 2);
end
-xlabel('t'); ylabel('kv / 2 kp^{0.5}');
+xlabel('t'); ylabel('Damping ratio');
-pause;
-close all;
+%pause;
+%close all;
diff --git a/demo_testLQR03.m b/demo_testLQR03.m
index 45a9ca890a2998b0792c4a845d6531689c0b611b..294ea3183bf2e7f11f7f38f37625ec1fdd891816 100644
--- a/demo_testLQR03.m
+++ b/demo_testLQR03.m
@@ -1,5 +1,5 @@
function demo_testLQR03
-% Comaprison of linear quadratic regulators with finite and infinite time horizons
+% Comparison of linear quadratic regulators with finite and infinite time horizons
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
diff --git a/demo_trajGMM01.m b/demo_trajGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..4ec4daaddbe2dc446133c080a31d08a743424c2f
--- /dev/null
+++ b/demo_trajGMM01.m
@@ -0,0 +1,189 @@
+function demo_trajGMM01
+%Example of trajectory synthesis with a GMM with dynamic features (trajectory GMM)
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 5; %Number of components in the GMM
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
+model.nbVar = model.nbVarPos * model.nbDeriv;
+model.dt = 1; %Time step (without rescaling, large values such as 1 has the advantage of creating clusers based on position information)
+nbSamples = 4; %Number of demonstrations
+nbD = 200; %Number of datapoints in a trajectory
+
+[PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples); %Construct PHI operator (big sparse matrix)
+
+
+%% Load dataset
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+load('data/dataTrajGMM01.mat');
+%load('data/dataTrajGMM02.mat'); %periodic motion
+%load('data/dataTrajGMM03.mat'); %motion with options
+%load('data/dataTrajGMM04.mat'); %partial demonstrations
+
+%Resampling of dataset
+nbD0 = size(Data,2)/nbSamples; %Original number of datapoints in a trajectory
+DataTmp = [];
+for n=1:nbSamples
+ DataTmp = [DataTmp spline(1:nbD0, Data(2:model.nbVarPos+1,(n-1)*nbD0+1:n*nbD0), linspace(1,nbD0,nbD))];
+end
+Data = DataTmp;
+
+%Re-arrange data in vector form
+x = reshape(Data(:,1:T), model.nbVarPos*T, 1) * 1E2; %Scale data to avoid numerical computation problem
+zeta = PHI*x; %y is for example [x1(1), x2(1), x1d(1), x2d(1), x1(2), x2(2), x1d(2), x2d(2), ...], see Eq. (2.4.5) in doc/TechnicalReport.pdf
+Data = reshape(zeta, model.nbVarPos*model.nbDeriv, T); %Include derivatives in Data
+
+
+%% Parameters estimation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Initialization with kmeans
+model = init_GMM_kmeans(Data, model);
+
+% %Initialization with homogeneous time intervals
+% DataTS = [repmat([1:T1]*model.dt,1,nbSamples); Data];
+% modelTmp = init_GMM_timeBased(DataTS, model);
+% model.Mu = modelTmp.Mu(2:end,:);
+% model.Sigma = modelTmp.Sigma(2:end,2:end,:);
+% model.Priors = modelTmp.Priors;
+
+%Refinement of parameters
+[model, GAMMA2] = EM_GMM(Data, model);
+%[model, GAMMA2] = EM_MFA(Data, model);
+%[model, GAMMA2] = EM_MPPCA(Data, model);
+%[model, GAMMA2] = EM_HDGMM(Data, model);
+
+%Precomputation of inverses (optional)
+for i=1:model.nbStates
+ model.invSigma(:,:,i) = inv(model.Sigma(:,:,i));
+end
+
+
+%% Reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+for n=1:1 %nbSamples
+ %Compute best path for the n-th demonstration
+ [~,r(n).q] = max(GAMMA2(:,(n-1)*T1+1:n*T1),[],1); %works also for nbStates=1
+
+ %Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (2.4.8) in doc/TechnicalReport.pdf
+ MuQ = reshape(model.Mu(:,r(n).q), model.nbVar*T1, 1);
+ %MuQ = zeros(model.nbVar*T1,1);
+ SigmaQ = zeros(model.nbVar*T1);
+ for t=1:T1
+ id = (t-1)*model.nbVar+1:t*model.nbVar;
+ %MuQ(id) = model.Mu(:,r(n).q(t));
+ SigmaQ(id,id) = model.Sigma(:,:,r(n).q(t));
+ end
+ %SigmaQ can alternatively be computed with:
+ %r(n).SigmaQ = (kron(ones(T1,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,r(1).q), model.nbVar, model.nbVar*T1)) .* kron(eye(T1), ones(model.nbVar));
+
+
+ %Least squares computation method 1 (using lscov Matlab function), see Eq. (2.4.11) in doc/TechnicalReport.pdf
+ %%%%%%%%%%%%%%%%%%%
+ [zeta,~,mse,S] = lscov(PHI1, MuQ, SigmaQ,'chol'); %Retrieval of data with weighted least squares solution
+ r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting
+
+
+% %Least squares computation method 2 (most readable but not optimized), see Eq. (2.4.11) in doc/TechnicalReport.pdf
+% %%%%%%%%%%%%%%%%%%%
+% PHIinvSigmaQ = PHI1' / SigmaQ;
+% Rq = PHIinvSigmaQ * PHI1;
+% rq = PHIinvSigmaQ * MuQ;
+% zeta = Rq \ rq; %Can also be computed with c = lscov(Rq, rq)
+% r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting
+% %Covariance Matrix computation of ordinary least squares estimate, see Eq. (2.4.12) in doc/TechnicalReport.pdf
+% mse = (MuQ'*inv(SigmaQ)*MuQ - rq'*inv(Rq)*rq) ./ ((model.nbVar-model.nbVarPos)*T1);
+% S = inv(Rq) * mse;
+
+
+% %Least squares computation method 3 (efficient computation using Cholesky and QR decompositions, inspired by lscov code)
+% %%%%%%%%%%%%%%%%%%%
+% T = chol(SigmaQ)'; %SigmaQ=T*T'
+% TA = T \ PHI1;
+% TMuQ = T \ MuQ;
+% [Q, R, perm] = qr(TA,0); %PHI1(:,perm)=Q*R
+% z = Q' * TMuQ;
+% zeta(perm,:) = R \ z; %zeta=(TA'*TA)\(TA'*TMuQ), see Eq. (2.4.11) in doc/TechnicalReport.pdf
+% r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting
+% %Covariance Matrix computation of ordinary least squares estimate
+% err = TMuQ - Q*z;
+% mse = err'*err ./ (model.nbVar*T1 - model.nbVarPos*T1);
+% Rinv = R \ eye(model.nbVarPos*T1);
+% S(perm,perm) = Rinv*Rinv' .* mse; %See Eq. (2.4.12) in doc/TechnicalReport.pdf
+
+
+ %Rebuild covariance by reshaping S, see Eq. (2.4.12) in doc/TechnicalReport.pdf
+ for t=1:T1
+ id = (t-1)*model.nbVarPos+1:t*model.nbVarPos;
+ r(n).expSigma(:,:,t) = S(id,id) * T1;
+ end
+
+end %nbSamples
+
+
+%% Plot timeline
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 600 600]);
+for m=1:model.nbVarPos
+ limAxes = [1, T1, min(Data(m,:))-1E0, max(Data(m,:))+1E0];
+ subplot(model.nbVarPos,1,m); hold on;
+ for n=1:1 %nbSamples
+ msh=[]; x0=[];
+ for t=1:T1-1
+ if size(msh,2)==0
+ msh(:,1) = [t; model.Mu(m,r(n).q(t))];
+ end
+ if t==T1-1 || r(n).q(t+1)~=r(n).q(t)
+ msh(:,2) = [t+1; model.Mu(m,r(n).q(t))];
+ sTmp = model.Sigma(m,m,r(n).q(t))^.5;
+ msh2 = [msh(:,1)+[0;sTmp], msh(:,2)+[0;sTmp], msh(:,2)-[0;sTmp], msh(:,1)-[0;sTmp], msh(:,1)+[0;sTmp]];
+ patch(msh2(1,:), msh2(2,:), [.85 .85 .85],'edgecolor',[.7 .7 .7]);
+ plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',[.7 .7 .7]);
+ plot([msh(1,1) msh(1,1)], limAxes(3:4), ':','linewidth',1,'color',[.7 .7 .7]);
+ x0 = [x0 msh];
+ msh=[];
+ end
+ end
+ msh = [1:T1, T1:-1:1; r(n).Data(m,:)-squeeze(r(n).expSigma(m,m,:).^.5)'*1, fliplr(r(n).Data(m,:)+squeeze(r(n).expSigma(m,m,:).^.5)'*1)];
+ patch(msh(1,:), msh(2,:), [1 .4 .4],'edgecolor',[1 .7 .7],'edgealpha',.8,'facealpha',.8);
+ end
+ for n=1:nbSamples
+ plot(1:T1, Data(m,(n-1)*T1+1:n*T1), '-','lineWidth',2,'color',[.2 .2 .2]);
+ end
+ for n=1:1
+ plot(1:T1, r(n).Data(m,:), '-','lineWidth',2.5,'color',[.8 0 0]);
+ end
+
+ set(gca,'xtick',[],'ytick',[]);
+ xlabel('$t$','interpreter','latex','fontsize',18);
+ ylabel(['$x_' num2str(m) '$'],'interpreter','latex','fontsize',18);
+ axis(limAxes);
+end
+
+%% Plot 2D
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+if model.nbVarPos>1
+ figure('position',[620 10 600 600]); hold on;
+ for n=1:1 %nbSamples
+ plotGMM(r(n).Data([1,2],:), r(n).expSigma([1,2],[1,2],:)*2, [1 .2 .2]);
+ end
+ plotGMM(model.Mu([1,2],:), model.Sigma([1,2],[1,2],:)*2, [.5 .5 .5],.8);
+ for n=1:nbSamples
+ plot(Data(1,(n-1)*T1+1:n*T1), Data(2,(n-1)*T1+1:n*T1), '-','lineWidth',2,'color',[.2 .2 .2]); %-0.2+0.8*(n-1)/(nbSamples-1)
+ end
+ for n=1:1
+ plot(r(n).Data(1,:), r(n).Data(2,:), '-','lineWidth',2.5,'color',[.8 0 0]);
+ end
+ set(gca,'xtick',[],'ytick',[]); axis equal; axis square;
+ xlabel(['$x_1$'],'interpreter','latex','fontsize',18);
+ ylabel(['$x_2$'],'interpreter','latex','fontsize',18);
+end
+
+%print('-dpng','graphs/demo_trajGMM01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_trajTPGMM01.m b/demo_trajTPGMM01.m
new file mode 100644
index 0000000000000000000000000000000000000000..2ddf6e5e98a1ac6cbfecf2ef9a0ef6d2f4074289
--- /dev/null
+++ b/demo_trajTPGMM01.m
@@ -0,0 +1,200 @@
+function demo_trajTPGMM01
+%Example of TP-GMM with trajectory GMM encoding
+%Sylvain Calinon, 2015
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
+model.dt = 0.01; %Time step
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+
+%% 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
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory
+% multiplied by the number of demonstrations.
+load('data/DataLQR01.mat');
+
+%Compute derivatives
+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 3rd order tensor data with XHAT instead of X
+model.nbVar = model.nbVarPos * model.nbDeriv;
+Data = zeros(model.nbVar, model.nbFrames, nbD);
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ for k=1:model.nbDeriv-1
+ DataTmp = [DataTmp; s(n).Data0(2:end,:)*D^k]; %Compute derivatives
+ end
+ for m=1:model.nbFrames
+ s(n).p(m).b = [s(n).p(m).b(2:end); zeros((model.nbDeriv-1)*model.nbVarPos,1)];
+ s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A(2:end,2:end));
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
+ end
+end
+
+%Construct PHI operator (big sparse matrix)
+[PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples);
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-GMM with EM:');
+%model = init_tensorGMM_timeBased(Data, model); %Initialization
+model = init_tensorGMM_kmeans(Data, model); %Initialization
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions...');
+%DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+ %Products of linearly transformed Gaussians
+ for i=1:model.nbStates
+ SigmaTmp = zeros(model.nbVar);
+ MuTmp = zeros(model.nbVar,1);
+ for m=1:model.nbFrames
+ MuP = s(n).p(m).A * model.Mu(:,m,i) + s(n).p(m).b;
+ SigmaP = s(n).p(m).A * model.Sigma(:,:,m,i) * s(n).p(m).A';
+ SigmaTmp = SigmaTmp + inv(SigmaP);
+ MuTmp = MuTmp + SigmaP\MuP;
+ end
+ r(n).Sigma(:,:,i) = inv(SigmaTmp);
+ r(n).Mu(:,i) = r(n).Sigma(:,:,i) * MuTmp;
+ end
+ %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q
+ [~,r(n).q] = max(model.Pix(:,(n-1)*T1+1:n*T1),[],1); %works also for nbStates=1
+ r(n).MuQ = reshape(r(n).Mu(:,r(n).q), model.nbVarPos*model.nbDeriv*T1, 1);
+ r(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1);
+ for t=1:T1
+ id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv;
+ r(n).SigmaQ(id1,id1) = r(n).Sigma(:,:,r(n).q(t));
+ end
+ %Retrieval of data with trajectory GMM
+ PHIinvSigmaQ = PHI1'/r(n).SigmaQ;
+ Rq = PHIinvSigmaQ * PHI1;
+ rq = PHIinvSigmaQ * r(n).MuQ;
+ r(n).Data = reshape(Rq\rq, model.nbVarPos, T1); %Reshape data for plotting
+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
+ %GMM products
+ for i=1:model.nbStates
+ SigmaTmp = zeros(model.nbVar);
+ MuTmp = zeros(model.nbVar,1);
+ for m=1:model.nbFrames
+ MuP = rnew(n).p(m).A * model.Mu(:,m,i) + rnew(n).p(m).b;
+ SigmaP = rnew(n).p(m).A * model.Sigma(:,:,m,i) * rnew(n).p(m).A';
+ SigmaTmp = SigmaTmp + inv(SigmaP);
+ MuTmp = MuTmp + SigmaP\MuP;
+ end
+ rnew(n).Sigma(:,:,i) = inv(SigmaTmp);
+ rnew(n).Mu(:,i) = rnew(n).Sigma(:,:,i) * MuTmp;
+ end
+ %Create single Gaussian N(MuQ,SigmaQ) based on state sequence q
+ [~,rnew(n).q] = max(model.Pix(:,1:T1),[],1); %works also for nbStates=1
+ rnew(n).MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVarPos*model.nbDeriv*T1, 1);
+ rnew(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1);
+ for t=1:T1
+ id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv;
+ rnew(n).SigmaQ(id1,id1) = rnew(n).Sigma(:,:,rnew(n).q(t));
+ end
+ %Retrieval of data with trajectory GMM
+ PHIinvSigmaQ = PHI1'/rnew(n).SigmaQ;
+ Rq = PHIinvSigmaQ * PHI1;
+ rq = PHIinvSigmaQ * rnew(n).MuQ;
+ rnew(n).Data = reshape(Rq\rq, model.nbVarPos, T1); %Reshape data for plotting
+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
+for n=1:nbSamples
+ %Plot Gaussians
+ plotGMM(r(n).Mu(1:2,:,1), r(n).Sigma(1:2,1:2,:,1), [.5 .5 .5],.8);
+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
+for n=1:nbRepros
+ %Plot Gaussians
+ plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5],.8);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_trajTPGMM01.png');
+%pause;
+%close all;
+
+
diff --git a/graphs/demo_GMM01.png b/graphs/demo_GMM01.png
new file mode 100644
index 0000000000000000000000000000000000000000..a22da9d51ef08316f9c850e123cd7e6bd1b313bc
Binary files /dev/null and b/graphs/demo_GMM01.png differ
diff --git a/m_fcts/EM_GMM.m b/m_fcts/EM_GMM.m
index 710d182b2fbbb041e60973617ae951189c6a0dcb..5492118cd25b0131a93810804f76cd2d7f316c6b 100644
--- a/m_fcts/EM_GMM.m
+++ b/m_fcts/EM_GMM.m
@@ -1,21 +1,8 @@
function [model, GAMMA2, LL] = EM_GMM(Data, model)
-% Training of a Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
+% Training of a Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
%
% Author: Sylvain Calinon, 2014
-% http://programming-by-demonstration.org/lib/
-%
-% This source code is given for free! In exchange, please cite the following
-% reference in any academic publication that uses this code or part of it:
-%
-% @article{Calinon07SMC,
-% author="Calinon, S. and Guenter, F. and Billard, A. G.",
-% title="On Learning, Representing and Generalizing a Task in a Humanoid Robot",
-% journal="{IEEE} Trans. on Systems, Man and Cybernetics, Part {B}",
-% year="2007",
-% volume="37",
-% number="2",
-% pages="286--298",
-% }
+% http://programming-by-demonstration.org/SylvainCalinon
%Parameters of the EM algorithm
nbMinSteps = 5; %Minimum number of iterations allowed
@@ -23,43 +10,50 @@ nbMaxSteps = 100; %Maximum number of iterations allowed
maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
nbData = size(Data,2);
-diagRegularizationFactor = 1E-6;
+%diagRegularizationFactor = 1E-6; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+diagRegularizationFactor = 1E-4; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
for nbIter=1:nbMaxSteps
- fprintf('.');
- %E-step
- [L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
- GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
- %M-step
- for i=1:model.nbStates
- %Update Priors
- model.Priors(i) = sum(GAMMA(i,:)) / nbData;
- %Update Mu
+ fprintf('.');
+
+ %E-step
+ [L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+
+ %M-step
+ for i=1:model.nbStates
+ %Update Priors, see Eq. (2.0.6) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(GAMMA(i,:)) / nbData;
+
+ %Update Mu, see Eq. (2.0.7) in doc/TechnicalReport.pdf
model.Mu(:,i) = Data * GAMMA2(i,:)';
- %Update Sigma (regularization term is optional)
+
+ %Update Sigma, see Eq. (2.0.8) in doc/TechnicalReport.pdf (regularization term is optional, see Eq. (2.1.2))
DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
- end
- %Compute average log-likelihood
- LL(nbIter) = sum(log(sum(L,1))) / nbData;
- %Stop the algorithm if EM converged (small change of LL)
- if nbIter>nbMinSteps
- if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
- disp(['EM converged after ' num2str(nbIter) ' iterations.']);
- return;
- end
- end
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(L,1))) / nbData;
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
end
-disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [L, GAMMA] = computeGamma(Data, model)
- L = zeros(model.nbStates,size(Data,2));
- for i=1:model.nbStates
- L(i,:) = model.Priors(i) * gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i));
- end
- GAMMA = L ./ repmat(sum(L,1)+realmin,model.nbStates,1);
+%See Eq. (2.0.5) in doc/TechnicalReport.pdf
+L = zeros(model.nbStates,size(Data,2));
+for i=1:model.nbStates
+ L(i,:) = model.Priors(i) * gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i));
+end
+GAMMA = L ./ repmat(sum(L,1)+realmin, model.nbStates, 1);
end
diff --git a/m_fcts/EM_HDGMM.m b/m_fcts/EM_HDGMM.m
new file mode 100644
index 0000000000000000000000000000000000000000..d9f447f01f88f86a40f0f7c0f49e68784ccef19d
--- /dev/null
+++ b/m_fcts/EM_HDGMM.m
@@ -0,0 +1,68 @@
+function [model, GAMMA2] = EM_HDGMM(Data, model)
+%EM for High Dimensional Data Clustering (HDDC, HD-GMM) model proposed by Bouveyron (2007)
+%Sylvain Calinon, 2015
+
+%Parameters of the EM iterations
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,2);
+
+diagRegularizationFactor = 1E-8; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+%EM loop
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [Lik, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2), 1, nbData);
+
+ %M-step
+ %Update Priors, see Eq. (2.0.6) in doc/TechnicalReport.pdf
+ model.Priors = sum(GAMMA,2)/nbData;
+
+ %Update Mu, see Eq. (2.0.7) in doc/TechnicalReport.pdf
+ model.Mu = Data * GAMMA2';
+
+ %Update factor analyser params
+ for i=1:model.nbStates
+ %Compute covariance
+ DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
+ S(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+
+ %HDGMM update, see Eq. (2.2.2) in doc/TechnicalReport.pdf
+ [V,D] = eig(S(:,:,i));
+ [~,id] = sort(diag(D),'descend');
+% model.D(:,:,i) = D(id(1:model.nbFA), id(1:model.nbFA));
+% model.V(:,:,i) = V(:, id(1:model.nbFA));
+ d = diag(D);
+ model.D(:,:,i) = diag([d(id(1:model.nbFA)); repmat(mean(d(id(model.nbFA+1:end))), model.nbVar-model.nbFA, 1)]);
+ model.V(:,:,i) = V(:,id);
+
+ %Reconstruct Sigma, see Eq. (2.2.1) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,i) = model.V(:,:,i) * model.D(:,:,i) * model.V(:,:,i)' + eye(model.nbVar) * diagRegularizationFactor;
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(Lik,1))) / nbData;
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA] = computeGamma(Data, model)
+%See Eq. (2.0.5) in doc/TechnicalReport.pdf
+Lik = zeros(model.nbStates,size(Data,2));
+for i=1:model.nbStates
+ Lik(i,:) = model.Priors(i) * gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i));
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, model.nbStates, 1);
+end
diff --git a/m_fcts/EM_MFA.m b/m_fcts/EM_MFA.m
new file mode 100644
index 0000000000000000000000000000000000000000..07ada7afbbae34b09d383f6a35d8a22e53c684bb
--- /dev/null
+++ b/m_fcts/EM_MFA.m
@@ -0,0 +1,87 @@
+function [model, GAMMA2] = EM_MFA(Data, model)
+%EM for Mixture of factor analysis
+%Implementation inspired by "Parsimonious Gaussian Mixture Models" by McNicholas and Murphy, Appendix 8, p.17 (UUU version)
+%Sylvain Calinon, 2015
+
+%Parameters of the EM iterations
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,2);
+
+diagRegularizationFactor = 1E-6; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+% %Circular initialization of the MFA parameters
+% Itmp = eye(model.nbVar)*1E-2;
+% model.P = repmat(Itmp, [1 1 model.nbStates]);
+% model.L = repmat(Itmp(:,1:model.nbFA), [1 1 model.nbStates]);
+
+%Initialization of the MFA parameters from eigendecomposition estimate
+for i=1:model.nbStates
+ model.P(:,:,i) = diag(diag(model.Sigma(:,:,i))); %Dimension-wise variance
+ [V,D] = eig(model.Sigma(:,:,i)-model.P(:,:,i));
+ [~,id] = sort(diag(D),'descend');
+ V = V(:,id)*D(id,id).^.5;
+ model.L(:,:,i) = V(:,1:model.nbFA); %->Sigma=LL'+P
+end
+for nbIter=1:nbMaxSteps
+ for i=1:model.nbStates
+ %Update B,L,P
+ B(:,:,i) = model.L(:,:,i)' / (model.L(:,:,i) * model.L(:,:,i)' + model.P(:,:,i));
+ model.L(:,:,i) = model.Sigma(:,:,i) * B(:,:,i)' / (eye(model.nbFA) - B(:,:,i) * model.L(:,:,i) + B(:,:,i) * model.Sigma(:,:,i) * B(:,:,i)');
+ model.P(:,:,i) = diag(diag(model.Sigma(:,:,i) - model.L(:,:,i) * B(:,:,i) * model.Sigma(:,:,i)));
+ end
+end
+
+%EM loop
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+ %E-step
+ [Lik, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+
+ %M-step
+ %Update Priors, see Eq. (2.2.10) in doc/TechnicalReport.pdf
+ model.Priors = sum(GAMMA,2) / nbData;
+
+ %Update Mu, see Eq. (2.2.11) in doc/TechnicalReport.pdf
+ model.Mu = Data * GAMMA2';
+
+ %Update factor analysers parameters
+ for i=1:model.nbStates
+ %Compute covariance, see Eq. (2.2.15) in doc/TechnicalReport.pdf
+ DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
+ S(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+
+ %Update B, see Eq. (2.2.16) in doc/TechnicalReport.pdf
+ B(:,:,i) = model.L(:,:,i)' / (model.L(:,:,i) * model.L(:,:,i)' + model.P(:,:,i));
+ %Update Lambda, see Eq. (2.2.12) in doc/TechnicalReport.pdf
+ model.L(:,:,i) = S(:,:,i) * B(:,:,i)' / (eye(model.nbFA) - B(:,:,i) * model.L(:,:,i) + B(:,:,i) * S(:,:,i) * B(:,:,i)');
+ %Update Psi, see Eq. (2.2.13) in doc/TechnicalReport.pdf
+ model.P(:,:,i) = diag(diag(S(:,:,i) - model.L(:,:,i) * B(:,:,i) * S(:,:,i))) + eye(model.nbVar) * diagRegularizationFactor;
+
+ %Reconstruct Sigma, see Eq. (2.2.4) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,i) = model.L(:,:,i) * model.L(:,:,i)' + model.P(:,:,i);
+ end
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(Lik,1))) / nbData;
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA] = computeGamma(Data, model)
+%See Eq. (2.2.9) in doc/TechnicalReport.pdf
+Lik = zeros(model.nbStates,size(Data,2));
+for i=1:model.nbStates
+ Lik(i,:) = model.Priors(i) * gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i));
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, model.nbStates, 1);
+end
diff --git a/m_fcts/EM_MPPCA.m b/m_fcts/EM_MPPCA.m
new file mode 100644
index 0000000000000000000000000000000000000000..bbf6faff216dadb451d86c93ed18866bbd88097d
--- /dev/null
+++ b/m_fcts/EM_MPPCA.m
@@ -0,0 +1,79 @@
+function [model, GAMMA2] = EM_MPPCA(Data, model)
+%EM for mixture of probabilistic principal component analyzers,
+%inspired by "Mixtures of Probabilistic Principal Component Analysers" by Michael E. Tipping and Christopher M. Bishop
+%Sylvain Calinon, 2015
+
+%Parameters of the EM iterations
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,2);
+
+diagRegularizationFactor = 1E-6; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+%Initialization of the MPPCA parameters from eigendecomposition
+for i=1:model.nbStates
+ model.o(i) = trace(model.Sigma(:,:,i)) / model.nbVar;
+ [V,D] = eig(model.Sigma(:,:,i)-eye(model.nbVar)*model.o(i));
+ [~,id] = sort(diag(D),'descend');
+ V = V(:,id)*D(id,id).^.5;
+ model.L(:,:,i) = V(:,1:model.nbFA);
+end
+
+%EM loop
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [Lik, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+
+ %M-step
+ %Update Priors, see Eq. (2.2.10) in doc/TechnicalReport.pdf
+ model.Priors = sum(GAMMA,2) / nbData;
+
+ %Update Mu, see Eq. (2.2.11) in doc/TechnicalReport.pdf
+ model.Mu = Data * GAMMA2';
+
+ %Update factor analyser params
+ for i=1:model.nbStates
+ %Compute covariance, see Eq. (2.2.19) in doc/TechnicalReport.pdf
+ DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
+ S(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+
+ %Update M, see Eq. (2.2.20) in doc/TechnicalReport.pdf
+ M = eye(model.nbFA)*model.o(i) + model.L(:,:,i)' * model.L(:,:,i);
+ %Update Lambda, see Eq. (2.2.17) in doc/TechnicalReport.pdf
+ Lnew = S(:,:,i) * model.L(:,:,i) / (eye(model.nbFA)*model.o(i) + M \ model.L(:,:,i)' * S(:,:,i) * model.L(:,:,i));
+ %Update of sigma^2, see Eq. (2.2.21) in doc/TechnicalReport.pdf
+ model.o(i) = trace(S(:,:,i) - S(:,:,i) * model.L(:,:,i) / M * Lnew') / model.nbVar;
+ model.L(:,:,i) = Lnew;
+ %Update Psi, see Eq. (2.2.18) in doc/TechnicalReport.pdf
+ model.P(:,:,i) = eye(model.nbVar) * model.o(i);
+
+ %Reconstruct Sigma, see Eq. (2.2.4) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,i) = model.L(:,:,i) * model.L(:,:,i)' + model.P(:,:,i);
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(Lik,1))) / nbData;
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA] = computeGamma(Data, model)
+%See Eq. (2.0.5) in doc/TechnicalReport.pdf
+Lik = zeros(model.nbStates,size(Data,2));
+for i=1:model.nbStates
+ Lik(i,:) = model.Priors(i) * gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i));
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, model.nbStates, 1);
+end
diff --git a/m_fcts/EM_TPGMM.m b/m_fcts/EM_TPGMM.m
new file mode 100644
index 0000000000000000000000000000000000000000..6881a71d3cfce617714dfde4feefa3995b74fb18
--- /dev/null
+++ b/m_fcts/EM_TPGMM.m
@@ -0,0 +1,91 @@
+function model = EM_TPGMM(Data, model)
+% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
+% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to
+% external parameters represented in the form of candidate coordinate systems.
+%
+% Author: Sylvain Calinon, 2014
+% http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following reference in any academic publication that uses this code or part of it:
+%
+% @inproceedings{Calinon14ICRA,
+% 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"
+% }
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-5; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+diagRegularizationFactor = 1E-5; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+%diagRegularizationFactor = 0; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+
+ %M-step
+ for i=1:model.nbStates
+
+ %Update Priors, see Eq. (6.0.2) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData;
+
+ for m=1:model.nbFrames
+ %Matricization/flattening of tensor
+ DataMat=[];
+ DataMat(1:model.nbVars(m),:) = Data(1:model.nbVars(m),m,:);
+
+ %Update Mu, see Eq. (6.0.3) in doc/TechnicalReport.pdf
+ model.Mu(1:model.nbVars(m),m,i) = DataMat * GAMMA2(i,:)';
+
+ %Update Sigma (regularization term is optional), see Eq. (6.0.4) in doc/TechnicalReport.pdf
+ DataTmp = DataMat - repmat(model.Mu(1:model.nbVars(m),m,i),1,nbData);
+ model.Sigma(1:model.nbVars(m),1:model.nbVars(m),m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVars(m))*diagRegularizationFactor;
+ end
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(L,1))) / size(L,2);
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA, GAMMA0] = computeGamma(Data, model)
+%See Eq. (6.0.1) in doc/TechnicalReport.pdf
+nbData = size(Data, 3);
+Lik = ones(model.nbStates, nbData);
+GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ DataMat=[];
+ DataMat(:,:) = Data(1:model.nbVars(m),m,:); %Matricization/flattening of tensor
+ GAMMA0(i,m,:) = gaussPDF(DataMat, model.Mu(1:model.nbVars(m),m,i), model.Sigma(1:model.nbVars(m),1:model.nbVars(m),m,i));
+ Lik(i,:) = Lik(i,:) .* squeeze(GAMMA0(i,m,:))';
+ end
+ Lik(i,:) = Lik(i,:) * model.Priors(i);
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, size(Lik,1), 1);
+end
+
+
+
+
diff --git a/m_fcts/EM_stdPGMM.m b/m_fcts/EM_stdPGMM.m
new file mode 100644
index 0000000000000000000000000000000000000000..81948478376476993b5c238868f2a759a44ac6ef
--- /dev/null
+++ b/m_fcts/EM_stdPGMM.m
@@ -0,0 +1,127 @@
+function [model, s, LL] = EM_stdPGMM(s, model)
+% Training of a parametric Gaussian mixture model (PGMM) with expectation-maximization (EM).
+% The implementation follows the approach described by Wilson and Bobick (1999) "Parametric Hidden Markov
+% Models for Gesture Recognition", IEEE Trans. on Pattern Analysis and Machine Intelligence, with an
+% implementation applied to the special case of Gaussian mixture models (GMM).
+%
+% Author: Sylvain Calinon, 2013
+% http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following references in any academic publication that uses this code or part of it:
+%
+% @inproceedings{Calinon12Hum,
+% author="Calinon, S. and Li, Z. and Alizadeh, T. and Tsagarakis, N. G. and Caldwell, D. G.",
+% title="Statistical dynamical systems for skills acquisition in humanoids",
+% booktitle="Proc. {IEEE} Intl Conf. on Humanoid Robots ({H}umanoids)",
+% year="2012",
+% address="Osaka, Japan"
+% }
+% @article{Wilson99,
+% author="Wilson, A. D. and Bobick, A. F.",
+% title="Parametric Hidden {M}arkov Models for Gesture Recognition",
+% journal="{IEEE} Trans. on Pattern Analysis and Machine Intelligence",
+% year="1999",
+% volume="21",
+% number="9",
+% pages="884--900"
+% }
+%
+% The first reference presents an implementation of the approach described in the second reference, and
+% applies it to the special case of Gaussian mixture model (GMM).
+
+%Parameters of the EM algorithm
+nbMinSteps = 10; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-50; %Likelihood increase threshold to stop the algorithm
+
+diagRegularizationFactor = 1E-4; %Optional regularization term for the covariance update
+
+%Initialization of the parameters
+nbSamples = length(s);
+nbData=0;
+for n=1:nbSamples
+ nbData = nbData + s(n).nbData;
+end
+nbVarParams = size(s(1).OmegaMu,1);
+
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-STEP
+ [s, GAMMA] = computeGamma(s, model); %See 'computeGamma' function below
+
+ %M-STEP
+ for i=1:model.nbStates
+
+ %Update Priors, see Eq. (7.1.5) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(GAMMA(i,:))/nbData;
+
+ %Update Zmu, see Eq. (7.1.6) in doc/TechnicalReport.pdf
+ model.ZMu(:,:,i) = zeros(model.nbVar,nbVarParams);
+ sumTmp = zeros(nbVarParams,nbVarParams);
+ for n=1:nbSamples
+ model.ZMu(:,:,i) = model.ZMu(:,:,i) + (s(n).Data * diag(s(n).GAMMA(i,:)) * repmat(s(n).OmegaMu',s(n).nbData,1));
+ sumTmp = sumTmp + (s(n).OmegaMu*s(n).OmegaMu') * (sum(s(n).GAMMA(i,:)));
+ end
+ model.ZMu(:,:,i) = model.ZMu(:,:,i) * pinv(sumTmp + eye(nbVarParams)*diagRegularizationFactor); %Eq. (6) Wilson and Bobick
+
+ %Update Sigma, see Eq. (7.1.7) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,i) = zeros(model.nbVar);
+ sumTmp = 0;
+ for n=1:nbSamples
+ MuTmp = model.ZMu(:,:,i) * s(n).OmegaMu;
+ Data_tmp = s(n).Data - repmat(MuTmp,1,s(n).nbData);
+ model.Sigma(:,:,i) = model.Sigma(:,:,i) + Data_tmp * diag(s(n).GAMMA(i,:)) * Data_tmp';
+ sumTmp = sumTmp + sum(s(n).GAMMA(i,:));
+ end
+ model.Sigma(:,:,i) = (model.Sigma(:,:,i) + eye(model.nbVar)*diagRegularizationFactor) / sumTmp;
+
+ end
+
+ %Computes the average log-likelihood through the ALPHA scaling factors
+ LL(nbIter)=0; sz=0;
+ for n=1:nbSamples
+ LL(nbIter) = LL(nbIter) + sum(log(sum(s(n).GAMMA0,1)));
+ sz = sz + s(n).nbData;
+ end
+ LL(nbIter) = LL(nbIter) / sz;
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [s, GAMMA] = computeGamma(s, model)
+nbSamples = length(s);
+nbStates = size(model.Sigma,3);
+%Observation probabilities
+GAMMA0=[];
+for n=1:nbSamples
+ for i=1:nbStates
+ MuTmp = model.ZMu(:,:,i) * s(n).OmegaMu;
+ s(n).GAMMA0(i,:) = model.Priors(i) * gaussPDF(s(n).Data,MuTmp,model.Sigma(:,:,i));
+ end
+ GAMMA0 = [GAMMA0 s(n).GAMMA0];
+end
+%Normalization
+GAMMA = GAMMA0 ./ repmat(sum(GAMMA0,1)+realmin,size(GAMMA0,1),1);
+%Data reshape
+nTmp=1;
+for n=1:nbSamples
+ s(n).GAMMA = GAMMA(:,[nTmp:nTmp+size(s(n).GAMMA0,2)-1]);
+ nTmp = nTmp+size(s(n).GAMMA,2);
+end
+end
+
+
+
+
diff --git a/m_fcts/EM_tensorGMM.m b/m_fcts/EM_tensorGMM.m
index 188a2cf433377279555f3de1608daa46c9ab87b2..15073fa59f606e49300d1e6e0a1f1e6b10962de1 100644
--- a/m_fcts/EM_tensorGMM.m
+++ b/m_fcts/EM_tensorGMM.m
@@ -1,78 +1,90 @@
-function model = EM_tensorGMM(Data, model)
-% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
-% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to
-% external parameters represented in the form of candidate coordinate systems.
-%
-% Author: Sylvain Calinon, 2014
-% http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite
-% the following reference in any academic publication that uses this code or part of it:
-%
-% @inproceedings{Calinon14ICRA,
-% 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"
-% }
-
-%Parameters of the EM algorithm
-nbMinSteps = 5; %Minimum number of iterations allowed
-nbMaxSteps = 100; %Maximum number of iterations allowed
-maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
-nbData = size(Data,3);
-
-%diagRegularizationFactor = 1E-2;
-diagRegularizationFactor = 1E-4;
-
-for nbIter=1:nbMaxSteps
- fprintf('.');
- %E-step
- [L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below and Eq. (2.0.5) in doc/TechnicalReport.pdf
- GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
- %M-step
- for i=1:model.nbStates
- %Update Priors
- model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf
- for m=1:model.nbFrames
- %Matricization/flattening of tensor
- DataMat(:,:) = Data(:,m,:);
- %Update Mu
- model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf
- %Update Sigma (regularization term is optional)
- DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
- model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; %See Eq. (2.0.8) and (2.1.2) in doc/TechnicalReport.pdf
- end
- end
- %Compute average log-likelihood
- LL(nbIter) = sum(log(sum(L,1))) / size(L,2); %See Eq. (2.0.4) in doc/TechnicalReport.pdf
- %Stop the algorithm if EM converged (small change of LL)
- if nbIter>nbMinSteps
- if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
- disp(['EM converged after ' num2str(nbIter) ' iterations.']);
- return;
- end
- end
-end
-disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
-end
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function [L, GAMMA, GAMMA0] = computeGamma(Data, model)
-%See Eq. (2.0.5) in doc/TechnicalReport.pdf
-nbData = size(Data, 3);
-L = ones(model.nbStates, nbData);
-GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
-for m=1:model.nbFrames
- DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
- for i=1:model.nbStates
- GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
- L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))';
- end
-end
-%Normalization
-GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1);
-end
+function [model, GAMMA0] = EM_tensorGMM(Data, model)
+% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
+% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to
+% external parameters represented in the form of candidate coordinate systems.
+%
+% Author: Sylvain Calinon, 2014
+% http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following reference in any academic publication that uses this code or part of it:
+%
+% @inproceedings{Calinon14ICRA,
+% 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"
+% }
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-5; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+%diagRegularizationFactor = 1E-2; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+diagRegularizationFactor = 1E-8; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+ model.Pix = GAMMA2;
+
+ %M-step
+ for i=1:model.nbStates
+
+ %Update Priors, see Eq. (6.0.2) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData;
+
+ for m=1:model.nbFrames
+ %Matricization/flattening of tensor
+ DataMat(:,:) = Data(:,m,:);
+
+ %Update Mu, see Eq. (6.0.3) in doc/TechnicalReport.pdf
+ model.Mu(:,m,i) = DataMat * GAMMA2(i,:)';
+
+ %Update Sigma (regularization term is optional), see Eq. (6.0.4) in doc/TechnicalReport.pdf
+ DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
+ model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+ end
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(L,1))) / size(L,2);
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA, GAMMA0] = computeGamma(Data, model)
+%See Eq. (6.0.1) in doc/TechnicalReport.pdf
+nbData = size(Data, 3);
+Lik = ones(model.nbStates, nbData);
+GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
+ GAMMA0(i,m,:) = gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
+ Lik(i,:) = Lik(i,:) .* squeeze(GAMMA0(i,m,:))';
+ end
+ Lik(i,:) = Lik(i,:) * model.Priors(i);
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, size(Lik,1), 1);
+end
+
+
+
+
diff --git a/m_fcts/EM_tensorHDGMM.m b/m_fcts/EM_tensorHDGMM.m
new file mode 100644
index 0000000000000000000000000000000000000000..6390e9e3dca6f7a5da9b2f2c1249ef514beeb4aa
--- /dev/null
+++ b/m_fcts/EM_tensorHDGMM.m
@@ -0,0 +1,84 @@
+function model = EM_tensorHDGMM(Data, model)
+%Training of a task-parameterized high dimensional GMM with an expectation-maximization (EM) algorithm.
+%Sylvain Calinon, 2015
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-5; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+%diagRegularizationFactor = 1E-2; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+diagRegularizationFactor = 1E-10; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+%EM loop
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [Lik, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+ model.Pix = GAMMA2;
+
+ %M-step
+ for i=1:model.nbStates
+
+ %Update Priors, see Eq. (6.0.2) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData;
+
+ for m=1:model.nbFrames
+ %Matricization/flattening of tensor
+ DataMat(:,:) = Data(:,m,:);
+
+ %Update Mu, see Eq. (6.0.3) in doc/TechnicalReport.pdf
+ model.Mu(:,m,i) = DataMat * GAMMA2(i,:)';
+
+ %Compute covariance, see Eq. (2.2.15) in doc/TechnicalReport.pdf
+ DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
+ S(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar)*diagRegularizationFactor;
+
+ %HDGMM update, see Eq. (2.2.2) in doc/TechnicalReport.pdf
+ [V,D] = eig(S(:,:,m,i));
+ [~,id] = sort(diag(D),'descend');
+ d = diag(D);
+ model.D(:,:,m,i) = diag([d(id(1:model.nbFA)); repmat(mean(d(id(model.nbFA+1:end))), model.nbVar-model.nbFA, 1)]);
+ model.V(:,:,m,i) = V(:,id);
+
+ %Reconstruct Sigma, see Eq. (2.2.1) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,m,i) = model.V(:,:,m,i) * model.D(:,:,m,i) * model.V(:,:,m,i)' + eye(model.nbVar) * diagRegularizationFactor;
+ end
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(Lik,1))) / size(Lik,2);
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA, GAMMA0] = computeGamma(Data, model)
+%See Eq. (6.0.1) in doc/TechnicalReport.pdf
+nbData = size(Data, 3);
+Lik = ones(model.nbStates, nbData);
+GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
+ GAMMA0(i,m,:) = gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
+ Lik(i,:) = Lik(i,:) .* squeeze(GAMMA0(i,m,:))';
+ end
+ Lik(i,:) = Lik(i,:) * model.Priors(i);
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, size(Lik,1), 1);
+end
+
+
+
+
diff --git a/m_fcts/EM_tensorMFA.m b/m_fcts/EM_tensorMFA.m
new file mode 100644
index 0000000000000000000000000000000000000000..d1fcf07e7ac4945b6b9283be92cf1d4e7d13ed16
--- /dev/null
+++ b/m_fcts/EM_tensorMFA.m
@@ -0,0 +1,110 @@
+function model = EM_tensorMFA(Data, model)
+%Training of a task-parameterized mixture of factor analyzers (TP-MFA) with an expectation-maximization (EM) algorithm.
+%Sylvain Calinon, 2015
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-5; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+%diagRegularizationFactor = 1E-2; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+diagRegularizationFactor = 1E-10; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+% %Initialization of the MFA parameters
+% Itmp = eye(model.nbVar)*1E-2;
+% model.P = repmat(Itmp, [1 1 model.nbFrames model.nbStates]);
+% model.L = repmat(Itmp(:,1:model.nbFA), [1 1 model.nbFrames model.nbStates]);
+
+%Initialization of the MFA parameters
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ model.P(:,:,m,i) = diag(diag(model.Sigma(:,:,m,i)));
+ [V,D] = eig(model.Sigma(:,:,m,i)-model.P(:,:,m,i));
+ [~,id] = sort(diag(D),'descend');
+ V = V(:,id)*D(id,id).^.5;
+ model.L(:,:,m,i) = V(:,1:model.nbFA);
+ end
+end
+for nbIter=1:nbMaxSteps
+ for i=1:model.nbStates
+ for m=1:model.nbFrames
+ %Update B,L,P
+ B(:,:,m,i) = model.L(:,:,m,i)' / (model.L(:,:,m,i) * model.L(:,:,m,i)' + model.P(:,:,m,i));
+ model.L(:,:,m,i) = model.Sigma(:,:,m,i) * B(:,:,m,i)' / (eye(model.nbFA) - B(:,:,m,i) * model.L(:,:,m,i) + B(:,:,m,i) * model.Sigma(:,:,m,i) * B(:,:,m,i)');
+ model.P(:,:,m,i) = diag(diag(model.Sigma(:,:,m,i) - model.L(:,:,m,i) * B(:,:,m,i) * model.Sigma(:,:,m,i)));
+ end
+ end
+end
+
+%EM loop
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [Lik, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+ model.Pix = GAMMA2;
+
+ %M-step
+ for i=1:model.nbStates
+
+ %Update Priors, see Eq. (6.0.2) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData;
+
+ for m=1:model.nbFrames
+ %Matricization/flattening of tensor
+ DataMat(:,:) = Data(:,m,:);
+
+ %Update Mu, see Eq. (6.0.3) in doc/TechnicalReport.pdf
+ model.Mu(:,m,i) = DataMat * GAMMA2(i,:)';
+
+ %Compute covariance, see Eq. (2.2.15) in doc/TechnicalReport.pdf
+ DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
+ S(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar)*diagRegularizationFactor;
+
+ %Update B, see Eq. (2.2.16) in doc/TechnicalReport.pdf
+ B(:,:,m,i) = model.L(:,:,m,i)' / (model.L(:,:,m,i) * model.L(:,:,m,i)' + model.P(:,:,m,i));
+ %Update Lambda, see Eq. (2.2.12) in doc/TechnicalReport.pdf
+ model.L(:,:,m,i) = S(:,:,m,i) * B(:,:,m,i)' / (eye(model.nbFA) - B(:,:,m,i) * model.L(:,:,m,i) + B(:,:,m,i) * S(:,:,m,i) * B(:,:,m,i)');
+ %Update Psi, see Eq. (2.2.13) in doc/TechnicalReport.pdf
+ model.P(:,:,m,i) = diag(diag(S(:,:,m,i) - model.L(:,:,m,i) * B(:,:,m,i) * S(:,:,m,i))) + eye(model.nbVar)*diagRegularizationFactor;
+
+ %Reconstruct Sigma, see Eqs (2.2.4) and (6.0.4) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,m,i) = model.L(:,:,m,i) * model.L(:,:,m,i)' + model.P(:,:,m,i);
+ end
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(Lik,1))) / size(Lik,2);
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA, GAMMA0] = computeGamma(Data, model)
+%See Eq. (6.0.1) in doc/TechnicalReport.pdf
+nbData = size(Data, 3);
+Lik = ones(model.nbStates, nbData);
+GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
+ GAMMA0(i,m,:) = gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
+ Lik(i,:) = Lik(i,:) .* squeeze(GAMMA0(i,m,:))';
+ end
+ Lik(i,:) = Lik(i,:) * model.Priors(i);
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, size(Lik,1), 1);
+end
+
+
+
+
diff --git a/m_fcts/EM_tensorMPPCA.m b/m_fcts/EM_tensorMPPCA.m
new file mode 100644
index 0000000000000000000000000000000000000000..618e1baaadffc733ef20dffe9d95f172faeac005
--- /dev/null
+++ b/m_fcts/EM_tensorMPPCA.m
@@ -0,0 +1,99 @@
+function model = EM_tensorMPPCA(Data, model)
+%Training of a task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA)
+%with an expectation-maximization (EM) algorithm.
+%Sylvain Calinon, 2015
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-5; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+%diagRegularizationFactor = 1E-2; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+diagRegularizationFactor = 1E-10; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+%Initialization of the MPPCA parameters from eigendecomposition
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ model.o(m,i) = trace(model.Sigma(:,:,m,i)) / model.nbVar;
+ [V,D] = eig(model.Sigma(:,:,m,i)-eye(model.nbVar)*model.o(m,i));
+ [~,id] = sort(diag(D),'descend');
+ V = V(:,id)*D(id,id).^.5;
+ model.L(:,:,m,i) = V(:,1:model.nbFA);
+ end
+end
+
+%EM loop
+for nbIter=1:nbMaxSteps
+ fprintf('.');
+
+ %E-step
+ [Lik, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
+ GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+ model.Pix = GAMMA2;
+
+ %M-step
+ for i=1:model.nbStates
+
+ %Update Priors, see Eq. (6.0.2) in doc/TechnicalReport.pdf
+ model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData;
+
+ for m=1:model.nbFrames
+ %Matricization/flattening of tensor
+ DataMat(:,:) = Data(:,m,:);
+
+ %Update Mu, see Eq. (6.0.3) in doc/TechnicalReport.pdf
+ model.Mu(:,m,i) = DataMat * GAMMA2(i,:)';
+
+ %Compute covariance, see Eq. (2.2.15) in doc/TechnicalReport.pdf
+ DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
+ S(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar)*diagRegularizationFactor;
+
+ %Update M, see Eq. (2.2.20) in doc/TechnicalReport.pdf
+ M = eye(model.nbFA)*model.o(m,i) + model.L(:,:,m,i)' * model.L(:,:,m,i);
+ %Update Lambda, see Eq. (2.2.17) in doc/TechnicalReport.pdf
+ Lnew = S(:,:,m,i) * model.L(:,:,m,i) / (eye(model.nbFA)*model.o(m,i) + M \ model.L(:,:,m,i)' * S(:,:,m,i) * model.L(:,:,m,i));
+ %Update of sigma^2, see Eq. (2.2.21) in doc/TechnicalReport.pdf
+ model.o(m,i) = trace(S(:,:,m,i) - S(:,:,m,i) * model.L(:,:,m,i) / M * Lnew') / model.nbVar;
+ model.L(:,:,m,i) = Lnew;
+ %Update Psi, see Eq. (2.2.18) in doc/TechnicalReport.pdf
+ model.P(:,:,m,i) = eye(model.nbVar) * model.o(m,i);
+
+ %Reconstruct Sigma, see Eqs (2.2.4) and (6.0.4) in doc/TechnicalReport.pdf
+ model.Sigma(:,:,m,i) = model.L(:,:,m,i) * model.L(:,:,m,i)' + model.P(:,:,m,i);
+ end
+ end
+
+ %Compute average log-likelihood
+ LL(nbIter) = sum(log(sum(Lik,1))) / size(Lik,2);
+ %Stop the algorithm if EM converged (small change of LL)
+ if nbIter>nbMinSteps
+ if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+ disp(['EM converged after ' num2str(nbIter) ' iterations.']);
+ return;
+ end
+ end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [Lik, GAMMA, GAMMA0] = computeGamma(Data, model)
+%See Eq. (6.0.1) in doc/TechnicalReport.pdf
+nbData = size(Data, 3);
+Lik = ones(model.nbStates, nbData);
+GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+for i=1:model.nbStates
+ for m=1:model.nbFrames
+ DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
+ GAMMA0(i,m,:) = gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
+ Lik(i,:) = Lik(i,:) .* squeeze(GAMMA0(i,m,:))';
+ end
+ Lik(i,:) = Lik(i,:) * model.Priors(i);
+end
+GAMMA = Lik ./ repmat(sum(Lik,1)+realmin, size(Lik,1), 1);
+end
+
+
+
+
diff --git a/m_fcts/GMR.m b/m_fcts/GMR.m
index 956de0355d430f6370d4b3cdf65ad893c9409fa8..bcd48651048a262de9536e0e1b359eacff3c5b29 100644
--- a/m_fcts/GMR.m
+++ b/m_fcts/GMR.m
@@ -20,26 +20,26 @@ function [expData, expSigma, H] = GMR(model, DataIn, in, out)
nbData = size(DataIn,2);
nbVarOut = length(out);
-diagRegularizationFactor = 1E-8;
+diagRegularizationFactor = 1E-8; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
MuTmp = zeros(nbVarOut,model.nbStates);
expData = zeros(nbVarOut,nbData);
expSigma = zeros(nbVarOut,nbVarOut,nbData);
for t=1:nbData
- %Compute activation weight
- %See Eq. (3.0.5) in doc/TechnicalReport.pdf
+
+ %Compute activation weight, see Eq. (3.0.5) in doc/TechnicalReport.pdf
for i=1:model.nbStates
H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i));
end
- H(:,t) = H(:,t)/sum(H(:,t)+realmin);
- %Compute expected conditional means
- %See Eq. (3.0.8) in doc/TechnicalReport.pdf
+ H(:,t) = H(:,t) / sum(H(:,t)+realmin);
+
+ %Compute conditional means, see Eq. (3.0.8) in doc/TechnicalReport.pdf
for i=1:model.nbStates
MuTmp(:,i) = model.Mu(out,i) + model.Sigma(out,in,i)/model.Sigma(in,in,i) * (DataIn(:,t)-model.Mu(in,i));
expData(:,t) = expData(:,t) + H(i,t) * MuTmp(:,i);
end
- %Compute expected conditional covariances
- %See Eq. (3.0.14) in doc/TechnicalReport.pdf
+
+ %Compute conditional covariances, see Eq. (3.0.14) in doc/TechnicalReport.pdf
for i=1:model.nbStates
SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i);
expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)');
diff --git a/m_fcts/GPR.m b/m_fcts/GPR.m
new file mode 100644
index 0000000000000000000000000000000000000000..c7b92a48780f3c9d5527e14f2ca6c7972b232b56
--- /dev/null
+++ b/m_fcts/GPR.m
@@ -0,0 +1,55 @@
+function [yd, SigmaOut] = GPR(q, y, qd, p)
+%Gaussian process regression (GPR), see Eq. (7.1.2) in doc/TechnicalReport.pdf
+%Sylvain Calinon, 2015
+
+%Kernel parameters
+if nargin<4
+ %p(1)=1; p(2)=1; p(3)=1E-5;
+ p(1)=1; p(2)=1E-1; p(3)=1E-3;
+end
+
+diagRegularizationFactor = 1E-4; %Regularization term is optional, see Eq. (2.1.2) in doc/TechnicalReport.pdf
+
+% %Linear least-squares regression
+% vOut = y * (pinv(q)*vIn);
+
+% %Recenter data
+% qmean = mean(q,2);
+% q = q - repmat(qmean,1,size(q,2));
+% qd = qd - repmat(qmean,1,size(vIn,2));
+% ymean = mean(y,2);
+% y = y - repmat(ymean,1,size(q,2));
+
+%GPR with exp() kernel
+M = pdist2(q', q');
+Md = pdist2(qd', q');
+K = p(1) * exp(-p(2) * M.^2);
+Kd = p(1) * exp(-p(2) * Md.^2);
+invK = pinv(K + p(3) * eye(size(K)));
+
+%Output
+yd = (Kd * invK * y')'; % + repmat(ymean,1,size(qd,2));
+
+if nargout>1
+ SigmaOut = zeros(size(yd,1), size(yd,1), size(yd,2));
+
+% %Evaluate Sigma (as in Rasmussen 2006)
+% Mdd = pdist2(qd',qd');
+% Kdd = exp(-Mdd.^2);
+% S = Kdd - Kd * invK * Kd';
+% for t=1:size(yd,2)
+% SigmaOut(:,:,t) = eye(size(yd,1)) * S(t,t);
+% end
+
+ %Evaluate Sigma (as in GMR)
+ %nbSamples = size(y,2) / size(yd,2);
+ %yd = repmat(yd,1,nbSamples);
+ for t=1:size(yd,2)
+ W = diag(K(t,:) * invK);
+ ym = repmat(yd(:,t), 1, size(y,2));
+ %SigmaOut(:,:,t) = (y-yd) * W * (y-yd)' + eye(size(vOut,1))*diagRegularizationFactor;
+ SigmaOut(:,:,t) = (y-ym) * W * (y-ym)' + eye(size(yd,1))*diagRegularizationFactor;
+ end
+end
+
+
diff --git a/m_fcts/constructPHI.m b/m_fcts/constructPHI.m
new file mode 100644
index 0000000000000000000000000000000000000000..a16a87f11160a1b3e930618f5c8720633eff9c32
--- /dev/null
+++ b/m_fcts/constructPHI.m
@@ -0,0 +1,25 @@
+function [PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples)
+%Construct PHI operator (big sparse matrix) used in trajectory-GMM, see Eq. (2.4.5) in doc/TechnicalReport.pdf
+%Sylvain Calinon, 2015
+
+T1 = nbD; %Number of datapoints in a demonstration
+T = T1 * nbSamples; %Total number of datapoints
+op1D = zeros(model.nbDeriv);
+op1D(1,end) = 1;
+for i=2:model.nbDeriv
+ op1D(i,:) = (op1D(i-1,:) - circshift(op1D(i-1,:),[0,-1])) / model.dt;
+end
+op = zeros(T1*model.nbDeriv,T1);
+op((model.nbDeriv-1)*model.nbDeriv+1:model.nbDeriv*model.nbDeriv,1:model.nbDeriv) = op1D;
+PHI0 = zeros(T1*model.nbDeriv,T1);
+for t=0:T1-model.nbDeriv
+ PHI0 = PHI0 + circshift(op, [model.nbDeriv*t,t]);
+end
+%Handling of borders
+for i=1:model.nbDeriv-1
+ op(model.nbDeriv*model.nbDeriv+1-i,:)=0; op(:,i)=0;
+ PHI0 = PHI0 + circshift(op, [-i*model.nbDeriv,-i]);
+end
+%Application to multiple dimensions and multiple demonstrations
+PHI1 = kron(PHI0, eye(model.nbVarPos));
+PHI = kron(eye(nbSamples), PHI1);
\ No newline at end of file
diff --git a/m_fcts/estimateAttractorPath.m b/m_fcts/estimateAttractorPath.m
index 47b67589dde6eb3d904ed8be5157fd3297575165..eeaaf74e35b04b3df4dff631ade0fbe5906465ea 100644
--- a/m_fcts/estimateAttractorPath.m
+++ b/m_fcts/estimateAttractorPath.m
@@ -24,7 +24,7 @@ out = in(end)+1:model.nbVar;
%% by using the GMM resulting from the product of linearly transformed Gaussians
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-[r.Mu, r.Sigma] = productTPGMM(model, r.p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
+[r.Mu, r.Sigma] = productTPGMM0(model, r.p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
r.Priors = model.Priors;
r.nbStates = model.nbStates;
diff --git a/m_fcts/init_GMM_kmeans.m b/m_fcts/init_GMM_kmeans.m
index 0b37444e10892af8f362dc70306e10dd3b92f34a..85ab8579f91504c2487fa18391b87d35a7f7cd0d 100644
--- a/m_fcts/init_GMM_kmeans.m
+++ b/m_fcts/init_GMM_kmeans.m
@@ -1,5 +1,6 @@
function model = init_GMM_kmeans(Data, model)
-% This function initializes the parameters of a Gaussian Mixture Model
+%
+% This function initializes the parameters of a Gaussian Mixture Model
% (GMM) by using k-means clustering algorithm.
%
% Inputs -----------------------------------------------------------------
@@ -9,23 +10,28 @@ function model = init_GMM_kmeans(Data, model)
% o Priors: 1 x K array representing the prior probabilities of the
% K GMM components.
% o Mu: D x K array representing the centers of the K GMM components.
-% o Sigma: D x D x K array representing the covariance matrices of the
+% o Sigma: D x D x K array representing the covariance matrices of the
% K GMM components.
+% Comments ---------------------------------------------------------------
+% o This function uses the 'kmeans' function from the MATLAB Statistics
+% toolbox. If you are using a version of the 'netlab' toolbox that also
+% uses a function named 'kmeans', please rename the netlab function to
+% 'kmeans_netlab.m' to avoid conflicts.
%
-% Author: Sylvain Calinon, 2014
-% http://programming-by-demonstration.org/lib/
+% Copyright (c) 2006 Sylvain Calinon, LASA Lab, EPFL, CH-1015 Lausanne,
+% Switzerland, http://lasa.epfl.ch
-nbVar = size(Data,1);
+[nbVar, nbData] = size(Data);
diagRegularizationFactor = 1E-2;
[Data_id, model.Mu] = kmeansClustering(Data, model.nbStates);
for i=1:model.nbStates
- idtmp = find(Data_id==i);
- model.Priors(i) = length(idtmp);
- model.Sigma(:,:,i) = cov([Data(:,idtmp) Data(:,idtmp)]');
- %Regularization term to avoid numerical instability
- model.Sigma(:,:,i) = model.Sigma(:,:,i) + eye(nbVar)*diagRegularizationFactor;
+ idtmp = find(Data_id==i);
+ model.Priors(i) = length(idtmp);
+ model.Sigma(:,:,i) = cov([Data(:,idtmp) Data(:,idtmp)]');
+ %Regularization term to avoid numerical instability
+ model.Sigma(:,:,i) = model.Sigma(:,:,i) + eye(nbVar)*diagRegularizationFactor;
end
model.Priors = model.Priors / sum(model.Priors);
diff --git a/m_fcts/init_GMM_timeBased.m b/m_fcts/init_GMM_timeBased.m
index 7dfe4c2ae3fc3e45238b410c6de8c76c4262e3ba..0677312ac265ebf01309e948adb1b8086d5a0ca7 100644
--- a/m_fcts/init_GMM_timeBased.m
+++ b/m_fcts/init_GMM_timeBased.m
@@ -1,5 +1,6 @@
function model = init_GMM_timeBased(Data, model)
-% This function initializes the parameters of a Gaussian Mixture Model
+%
+% This function initializes the parameters of a Gaussian Mixture Model
% (GMM) by using k-means clustering algorithm.
%
% Inputs -----------------------------------------------------------------
@@ -9,23 +10,29 @@ function model = init_GMM_timeBased(Data, model)
% o Priors: 1 x K array representing the prior probabilities of the
% K GMM components.
% o Mu: D x K array representing the centers of the K GMM components.
-% o Sigma: D x D x K array representing the covariance matrices of the
+% o Sigma: D x D x K array representing the covariance matrices of the
% K GMM components.
+% Comments ---------------------------------------------------------------
+% o This function uses the 'kmeans' function from the MATLAB Statistics
+% toolbox. If you are using a version of the 'netlab' toolbox that also
+% uses a function named 'kmeans', please rename the netlab function to
+% 'kmeans_netlab.m' to avoid conflicts.
%
-% Author: Sylvain Calinon, 2014
-% http://programming-by-demonstration.org/lib/
+% Copyright (c) 2006 Sylvain Calinon, LASA Lab, EPFL, CH-1015 Lausanne,
+% Switzerland, http://lasa.epfl.ch
-nbVar = size(Data,1);
-TimingSep = linspace(min(Data(1,:)), max(Data(1,:)), model.nbStates+1);
+[nbVar, nbData] = size(Data);
diagRegularizationFactor = 1E-2;
+TimingSep = linspace(min(Data(1,:)), max(Data(1,:)), model.nbStates+1);
+
for i=1:model.nbStates
- idtmp = find( Data(1,:)>=TimingSep(i) & Data(1,:)<TimingSep(i+1));
- model.Priors(i) = length(idtmp);
- model.Mu(:,i) = mean(Data(:,idtmp)');
- model.Sigma(:,:,i) = cov(Data(:,idtmp)');
- %Regularization term to avoid numerical instability
- model.Sigma(:,:,i) = model.Sigma(:,:,i) + eye(nbVar)*diagRegularizationFactor;
+ idtmp = find( Data(1,:)>=TimingSep(i) & Data(1,:)<TimingSep(i+1));
+ model.Priors(i) = length(idtmp);
+ model.Mu(:,i) = mean(Data(:,idtmp)');
+ model.Sigma(:,:,i) = cov(Data(:,idtmp)');
+ %Regularization term to avoid numerical instability
+ model.Sigma(:,:,i) = model.Sigma(:,:,i) + eye(nbVar)*diagRegularizationFactor;
end
model.Priors = model.Priors / sum(model.Priors);
diff --git a/m_fcts/init_TPGMM_kmeans.m b/m_fcts/init_TPGMM_kmeans.m
new file mode 100644
index 0000000000000000000000000000000000000000..c0ea9ff61817d892d0ee0a0851be6358010ce5c2
--- /dev/null
+++ b/m_fcts/init_TPGMM_kmeans.m
@@ -0,0 +1,31 @@
+function model = init_tensorGMM_kmeans(Data, model)
+% Initialization of the model with k-means.
+% Authors: Sylvain Calinon, Tohid Alizadeh, 2013
+% http://programming-by-demonstration.org/
+
+diagRegularizationFactor = 1E-4;
+
+DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
+
+%k-means clustering
+[Data_id, Mu] = kmeansClustering(DataAll, model.nbStates);
+
+for i=1:model.nbStates
+ idtmp = find(Data_id==i);
+ model.Priors(i) = length(idtmp);
+ Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor;
+end
+model.Priors = model.Priors / sum(model.Priors);
+
+%Reshape GMM parameters into a tensor
+for m=1:model.nbFrames
+ for i=1:model.nbStates
+ model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i);
+ model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
+ end
+end
+
+
+
+
+
diff --git a/m_fcts/init_TPGMM_timeBased.m b/m_fcts/init_TPGMM_timeBased.m
new file mode 100644
index 0000000000000000000000000000000000000000..2d427bf2fe4dd38ab4f1687b2a9267e5fffa43a0
--- /dev/null
+++ b/m_fcts/init_TPGMM_timeBased.m
@@ -0,0 +1,28 @@
+function model = init_TPGMM_timeBased(Data, model)
+% Author: Sylvain Calinon, 2014
+% http://programming-by-demonstration.org/SylvainCalinon
+
+diagRegularizationFactor = 1E-4;
+nbVar = size(Data,1); %nbVar is used instead of model.nbVar to be compatible with TPGMM with frames of different sizes
+
+DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
+
+TimingSep = linspace(min(DataAll(1,:)), max(DataAll(1,:)), model.nbStates+1);
+Mu = zeros(model.nbFrames*nbVar, model.nbStates);
+Sigma = zeros(model.nbFrames*nbVar, model.nbFrames*nbVar, model.nbStates);
+for i=1:model.nbStates
+ idtmp = find( DataAll(1,:)>=TimingSep(i) & DataAll(1,:)<TimingSep(i+1));
+ Mu(:,i) = mean(DataAll(:,idtmp),2);
+ Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor;
+ model.Priors(i) = length(idtmp);
+end
+model.Priors = model.Priors / sum(model.Priors);
+
+%Reshape GMM parameters into a tensor
+for m=1:model.nbFrames
+ for i=1:model.nbStates
+ model.Mu(:,m,i) = Mu((m-1)*nbVar+1:m*nbVar,i);
+ model.Sigma(:,:,m,i) = Sigma((m-1)*nbVar+1:m*nbVar,(m-1)*nbVar+1:m*nbVar,i);
+ end
+end
+
diff --git a/m_fcts/parseArgs.m b/m_fcts/parseArgs.m
new file mode 100644
index 0000000000000000000000000000000000000000..317d1e3941aadf817c70bceb1dadc82b5adb7a10
--- /dev/null
+++ b/m_fcts/parseArgs.m
@@ -0,0 +1,126 @@
+function ArgStruct=parseArgs(args,ArgStruct,varargin)
+% Helper function for parsing varargin.
+%
+%
+% ArgStruct=parseArgs(varargin,ArgStruct[,FlagtypeParams[,Aliases]])
+%
+% * ArgStruct is the structure full of named arguments with default values.
+% * Flagtype params is params that don't require a value. (the value will be set to 1 if it is present)
+% * Aliases can be used to map one argument-name to several argstruct fields
+%
+%
+% example usage:
+% --------------
+% function parseargtest(varargin)
+%
+% %define the acceptable named arguments and assign default values
+% Args=struct('Holdaxis',0, ...
+% 'SpacingVertical',0.05,'SpacingHorizontal',0.05, ...
+% 'PaddingLeft',0,'PaddingRight',0,'PaddingTop',0,'PaddingBottom',0, ...
+% 'MarginLeft',.1,'MarginRight',.1,'MarginTop',.1,'MarginBottom',.1, ...
+% 'rows',[],'cols',[]);
+%
+% %The capital letters define abrreviations.
+% % Eg. parseargtest('spacingvertical',0) is equivalent to parseargtest('sv',0)
+%
+% Args=parseArgs(varargin,Args, ... % fill the arg-struct with values entered by the user
+% {'Holdaxis'}, ... %this argument has no value (flag-type)
+% {'Spacing' {'sh','sv'}; 'Padding' {'pl','pr','pt','pb'}; 'Margin' {'ml','mr','mt','mb'}});
+%
+% disp(Args)
+%
+%
+%
+%
+% % Aslak Grinsted 2003
+
+Aliases={};
+FlagTypeParams='';
+
+if (length(varargin)>0)
+ FlagTypeParams=strvcat(varargin{1});
+ if length(varargin)>1
+ Aliases=varargin{2};
+ end
+end
+
+
+%---------------Get "numeric" arguments
+NumArgCount=1;
+while (NumArgCount<=size(args,2))&(~ischar(args{NumArgCount}))
+ NumArgCount=NumArgCount+1;
+end
+NumArgCount=NumArgCount-1;
+if (NumArgCount>0)
+ ArgStruct.NumericArguments={args{1:NumArgCount}};
+else
+ ArgStruct.NumericArguments={};
+end
+
+
+%--------------Make an accepted fieldname matrix (case insensitive)
+Fnames=fieldnames(ArgStruct);
+for i=1:length(Fnames)
+ name=lower(Fnames{i,1});
+ Fnames{i,2}=name; %col2=lower
+ AbbrevIdx=find(Fnames{i,1}~=name);
+ Fnames{i,3}=[name(AbbrevIdx) ' ']; %col3=abreviation letters (those that are uppercase in the ArgStruct) e.g. SpacingHoriz->sh
+ %the space prevents strvcat from removing empty lines
+ Fnames{i,4}=isempty(strmatch(Fnames{i,2},FlagTypeParams)); %Does this parameter have a value? (e.g. not flagtype)
+end
+FnamesFull=strvcat(Fnames{:,2});
+FnamesAbbr=strvcat(Fnames{:,3});
+
+if length(Aliases)>0
+ for i=1:length(Aliases)
+ name=lower(Aliases{i,1});
+ FieldIdx=strmatch(name,FnamesAbbr,'exact'); %try abbreviations (must be exact)
+ if isempty(FieldIdx)
+ FieldIdx=strmatch(name,FnamesFull); %&??????? exact or not?
+ end
+ Aliases{i,2}=FieldIdx;
+ AbbrevIdx=find(Aliases{i,1}~=name);
+ Aliases{i,3}=[name(AbbrevIdx) ' ']; %the space prevents strvcat from removing empty lines
+ Aliases{i,1}=name; %dont need the name in uppercase anymore for aliases
+ end
+ %Append aliases to the end of FnamesFull and FnamesAbbr
+ FnamesFull=strvcat(FnamesFull,strvcat(Aliases{:,1}));
+ FnamesAbbr=strvcat(FnamesAbbr,strvcat(Aliases{:,3}));
+end
+
+%--------------get parameters--------------------
+l=NumArgCount+1;
+while (l<=length(args))
+ a=args{l};
+ if ischar(a)
+ paramHasValue=1; % assume that the parameter has is of type 'param',value
+ a=lower(a);
+ FieldIdx=strmatch(a,FnamesAbbr,'exact'); %try abbreviations (must be exact)
+ if isempty(FieldIdx)
+ FieldIdx=strmatch(a,FnamesFull);
+ end
+ if (length(FieldIdx)>1) %shortest fieldname should win
+ [mx,mxi]=max(sum(FnamesFull(FieldIdx,:)==' ',2));
+ FieldIdx=FieldIdx(mxi);
+ end
+ if FieldIdx>length(Fnames) %then it's an alias type.
+ FieldIdx=Aliases{FieldIdx-length(Fnames),2};
+ end
+
+ if isempty(FieldIdx)
+ error(['Unknown named parameter: ' a])
+ end
+ for curField=FieldIdx' %if it is an alias it could be more than one.
+ if (Fnames{curField,4})
+ val=args{l+1};
+ else
+ val=1; %parameter is of flag type and is set (1=true)....
+ end
+ ArgStruct.(Fnames{curField,1})=val;
+ end
+ l=l+1+Fnames{FieldIdx(1),4}; %if a wildcard matches more than one
+ else
+ error(['Expected a named parameter: ' num2str(a)])
+ end
+end
+
diff --git a/m_fcts/plot2DArrow.m b/m_fcts/plot2DArrow.m
new file mode 100644
index 0000000000000000000000000000000000000000..a39b3a019a29408ff70dd032826eb7254e16f7b7
--- /dev/null
+++ b/m_fcts/plot2DArrow.m
@@ -0,0 +1,14 @@
+function h = plot2DArrow(pos,dir,col)
+%Sylvain Calinon, 2015
+
+h(1) = plot([pos(1) pos(1)+dir(1)], [pos(2) pos(2)+dir(2)], '-','linewidth',2,'color',col);
+sz = 8E-2;
+pos = pos+dir;
+if norm(dir)>sz
+ dir = dir/norm(dir);
+ prp = [dir(2); -dir(1)];
+ dir = dir*sz;
+ prp = prp*sz;
+ msh = [pos pos-dir-prp/2 pos-dir+prp/2 pos];
+ h(2) = patch(msh(1,:),msh(2,:),col,'edgecolor',col);
+end
\ No newline at end of file
diff --git a/m_fcts/plotArm.m b/m_fcts/plotArm.m
new file mode 100644
index 0000000000000000000000000000000000000000..61df059e3313edebe31a6427731d8085292aa4b9
--- /dev/null
+++ b/m_fcts/plotArm.m
@@ -0,0 +1,21 @@
+function h = plotArm(a, d, p, sz, facecolor, edgecolor)
+%Sylvain Calinon, 2014
+
+if nargin<4
+ sz = .05;
+end
+if nargin<5
+ facecolor = [.5,.5,.5];
+end
+if nargin<6
+ edgecolor = [1,1,1];
+end
+if size(p,1)==2
+ p = [p; -1];
+end
+
+h = plotArmBasis(p, sz, facecolor, edgecolor);
+for i=1:length(a)
+ [p, hTmp] = plotArmLink(sum(a(1:i)), d(i), p, sz, facecolor, edgecolor);
+ h = [h hTmp];
+end
diff --git a/m_fcts/plotArmBasis.m b/m_fcts/plotArmBasis.m
new file mode 100644
index 0000000000000000000000000000000000000000..8fa19ca4cd128e33fde3b507c5a928e489878b83
--- /dev/null
+++ b/m_fcts/plotArmBasis.m
@@ -0,0 +1,21 @@
+function h = plotArmBasis(p1, sz, facecol, edgecol)
+%Sylvain Calinon, 2014
+
+nbSegm = 30;
+sz = sz*1.2;
+
+t1 = linspace(0,pi,nbSegm-2);
+xTmp(1,:) = [sz*1.5 sz.*1.5*cos(t1) -sz*1.5];
+xTmp(2,:) = [-sz*1.2 sz.*1.5*sin(t1) -sz*1.2];
+xTmp(3,:) = [t1(1) t1 t1(end)];
+x = xTmp + repmat(p1,1,nbSegm);
+h = patch(x(1,:),x(2,:), facecol,'edgecolor',edgecol); %,'edgealpha',.9,'facealpha',.9
+
+xTmp2(1,:) = linspace(-sz*1.2,sz*1.2,5);
+xTmp2(2,:) = repmat(-sz*1.2,1,5);
+xTmp2(3,:) = zeros(1,5);
+x2 = xTmp2 + repmat(p1,1,5);
+x3 = xTmp2 + repmat(p1+[-0.5;-1;0]*sz,1,5);
+for i=1:5
+ h = [h patch([x2(1,i) x3(1,i)], [x2(2,i) x3(2,i)], facecol,'edgecolor',facecol)]; %,'edgealpha',.9,'facealpha',.9
+end
diff --git a/m_fcts/plotArmLink.m b/m_fcts/plotArmLink.m
new file mode 100644
index 0000000000000000000000000000000000000000..97c1fd675e2e7fe07bc5e1b89247447ec0518672
--- /dev/null
+++ b/m_fcts/plotArmLink.m
@@ -0,0 +1,19 @@
+function [p2, h] = plotArmLink(a1, d1, p1, sz, facecol, edgecol)
+%Sylvain Calinon, 2014
+
+nbSegm = 30;
+
+p1 = p1 + [0; 0; .1];
+t1 = linspace(0,-pi,nbSegm/2);
+t2 = linspace(pi,0,nbSegm/2);
+xTmp(1,:) = [sz.*sin(t1) d1+sz.*sin(t2)];
+xTmp(2,:) = [sz.*cos(t1) sz.*cos(t2)];
+xTmp(3,:) = zeros(1,nbSegm);
+R = [cos(a1) -sin(a1) 0; sin(a1) cos(a1) 0; 0 0 0];
+x = R*xTmp + repmat(p1,1,nbSegm);
+p2 = R*[d1;0;0] + p1;
+h = patch(x(1,:),x(2,:),x(3,:),facecol,'edgecolor',edgecol); %,'linewidth',1,,'facealpha',.9,'edgealpha',.9
+msh = [sin(linspace(0,2*pi,nbSegm)); cos(linspace(0,2*pi,nbSegm))]*sz*0.2;
+h = [h patch(msh(1,:)+p1(1), msh(2,:)+p1(2), repmat(p1(3),1,nbSegm), edgecol,'edgeColor',edgecol)]; %,'facealpha',.9,'edgealpha',.9
+h = [h patch(msh(1,:)+p2(1), msh(2,:)+p2(2), repmat(p2(3),1,nbSegm), edgecol,'edgeColor',edgecol)]; %,'facealpha',.9,'edgealpha',.9
+
diff --git a/m_fcts/plotBimanualRobot.m b/m_fcts/plotBimanualRobot.m
new file mode 100644
index 0000000000000000000000000000000000000000..cbc08537eafbc0328fc2afa1a2276fb8c5bf291e
--- /dev/null
+++ b/m_fcts/plotBimanualRobot.m
@@ -0,0 +1,84 @@
+function h = plotBimanualRobot(theta,coltmp)
+% This function plots the simulated robotic arm in 2D space.
+% Sylvain Calinon, 2014
+
+ global r1 r2;
+
+ %Right part
+ Rtmp = subs_Rr(theta);
+ Ttmp = subs_Tr(theta);
+ h = plotBodyBasis(Ttmp(:,1),coltmp);
+ h = [h plotBodyT(Rtmp(:,:,1),Ttmp(:,1),r1.L(1),'',coltmp)];
+ h = [h plotBodyLink(Rtmp(:,:,2),Ttmp(:,2),r1.L(2),'',coltmp)];
+ h = [h plotBodyLink(Rtmp(:,:,3),Ttmp(:,3),r1.L(3),'',coltmp)];
+
+ %Left part
+ Rtmp = subs_Rl(theta);
+ Ttmp = subs_Tl(theta);
+ h = [h plotBodyT(Rtmp(:,:,1),Ttmp(:,1),r2.L(1),'',coltmp)];
+ h = [h plotBodyLink(Rtmp(:,:,2),Ttmp(:,2),r2.L(2),'',coltmp)];
+ h = [h plotBodyLink(Rtmp(:,:,3),Ttmp(:,3),r2.L(3),'',coltmp)];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotBodyBasis(p1,coltmp)
+% This function plots the basis of the simulated robotic arm in 2D space.
+ nbSegm = 50;
+ t1 = linspace(0,pi,nbSegm-2);
+ xTmp(1,:) = [1.5 1.5*cos(t1) -1.5];
+ xTmp(2,:) = [-1.2 1.5*sin(t1) -1.2];
+ x = xTmp + repmat(p1,1,nbSegm);
+ h = patch(x(1,:),x(2,:),[1 1 1],'edgeColor',coltmp);
+
+ xTmp2(1,:) = linspace(-1.2,1.2,5);
+ xTmp2(2,:) = repmat(-1.2,1,5);
+ x2 = xTmp2 + repmat(p1,1,5);
+ x3 = xTmp2 + repmat(p1+[-.4;-.8],1,5);
+ for i=1:5
+ h = [h plot([x2(1,i) x3(1,i)],[x2(2,i) x3(2,i)],'-','color',coltmp)];
+ end
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotBodyLink(R,T,L,txt,coltmp)
+% This function plots an a link of the robotic arm in 2D space.
+ nbSegm = 50;
+ t1 = linspace(0,-pi,nbSegm/2);
+ t2 = linspace(pi,0,nbSegm/2);
+ xTmp(1,:) = [sin(t1) L+sin(t2)];
+ xTmp(2,:) = [cos(t1) cos(t2)];
+ x = R*xTmp + repmat(T,1,nbSegm);
+ T2=T+R*[L;0];
+ h(1) = fill(x(1,:),x(2,:),[1 1 1],'edgeColor',coltmp);
+ h(2) = rectangle('Position',[T(1)-.4,T(2)-.4,.8,.8], 'Curvature',[1,1], 'LineWidth',1,'LineStyle','-','facecolor',[.9 .9 .9],'edgeColor',coltmp);
+ h(3) = rectangle('Position',[T2(1)-.4,T2(2)-.4,.8,.8],'Curvature',[1,1], 'LineWidth',1,'LineStyle','-','facecolor',[.9 .9 .9],'edgeColor',coltmp);
+ ptxt = R*[L*0.7;0.8] + T;
+ h(4) = text(ptxt(1),ptxt(2),txt,'color',coltmp);
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotBodyT(R,T,L,txt,coltmp)
+% This function plots an a link of the robotic arm in 2D space.
+ nbSegm = 50;
+ t1 = linspace(0,-pi,nbSegm/2);
+ t2 = linspace(pi,0,nbSegm/2);
+ xTmp(1,:) = [sin(t1) L+sin(t2)];
+ xTmp(2,:) = [cos(t1) cos(t2)];
+
+ x = R*xTmp + repmat(T,1,nbSegm);
+ T2=T+R*[L;0];
+ h(1) = fill(x(1,:),x(2,:),[1 1 1],'edgeColor',coltmp);
+ h(2) = rectangle('Position',[T(1)-.4,T(2)-.4,.8,.8], 'Curvature',[1,1], 'LineWidth',1,'LineStyle','-','facecolor',[1 1 1],'edgeColor',coltmp);
+ ptxt = R*[L*0.7;0.8] + T;
+ h(3) = text(ptxt(1),ptxt(2),txt,'color',coltmp);
+
+ T = T2 + R * [0;-L/2];
+ R = R * [cos(pi/2) -sin(pi/2); sin(pi/2) cos(pi/2)];
+ x = R*xTmp + repmat(T,1,nbSegm);
+ T2=T+R*[L;0];
+ h(4) = fill(x(1,:),x(2,:),[1 1 1],'edgeColor',coltmp);
+ h(5) = rectangle('Position',[T(1)-.4,T(2)-.4,.8,.8], 'Curvature',[1,1], 'LineWidth',1,'LineStyle','-','facecolor',[1 1 1],'edgeColor',coltmp);
+ h(6) = rectangle('Position',[T2(1)-.4,T2(2)-.4,.8,.8],'Curvature',[1,1], 'LineWidth',1,'LineStyle','-','facecolor',[1 1 1],'edgeColor',coltmp);
+end
diff --git a/m_fcts/plotGMM.m b/m_fcts/plotGMM.m
index 9a3ca2f24ba60d0fd08d97cb2bc6eb9bb8dc7719..68e9a79535ad7e371c9e435f0041fe1728a62264 100644
--- a/m_fcts/plotGMM.m
+++ b/m_fcts/plotGMM.m
@@ -1,22 +1,31 @@
-function h = plotGMM(Mu, Sigma, color)
+function h = plotGMM(Mu, Sigma, color, valAlpha)
% This function displays the parameters of a Gaussian Mixture Model (GMM).
% Inputs -----------------------------------------------------------------
% o Mu: D x K array representing the centers of K Gaussians.
% o Sigma: D x D x K array representing the covariance matrices of K Gaussians.
% o color: 3 x 1 array representing the RGB color to use for the display.
+% o valAlpha: transparency factor (optional).
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
nbStates = size(Mu,2);
nbDrawingSeg = 35;
-lightcolor = min(color+0.3,1);
+lightcolor = min(color+0.5,1);
t = linspace(-pi, pi, nbDrawingSeg);
h=[];
for i=1:nbStates
- R = real(sqrtm(1.0.*Sigma(:,:,i)));
+ %R = real(sqrtm(1.0.*Sigma(:,:,i)));
+ [V,D] = eig(Sigma(:,:,i));
+ R = real(V*D.^.5);
X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg);
- h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)];
- h = [h plot(Mu(1,:), Mu(2,:), 'x', 'lineWidth', 2, 'markersize', 6, 'color', color)];
+ if nargin>3 %Plot with alpha transparency
+ h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color, 'facealpha', valAlpha,'edgealpha', valAlpha)];
+ MuTmp = [cos(t); sin(t)] * 0.006 + repmat(Mu(:,i),1,nbDrawingSeg);
+ h = [h patch(MuTmp(1,:), MuTmp(2,:), color, 'LineStyle', 'none', 'facealpha', valAlpha)];
+ else %Plot without transparency
+ h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)];
+ h = [h plot(Mu(1,:), Mu(2,:), '.', 'lineWidth', 2, 'markersize', 6, 'color', color)];
+ end
end
diff --git a/m_fcts/productTPGMM.m b/m_fcts/productTPGMM.m
index 581dd36d5339b814196f235ae14e451f5308267f..c81881f3b85a37645f19f2390d2446091894a700 100644
--- a/m_fcts/productTPGMM.m
+++ b/m_fcts/productTPGMM.m
@@ -1,19 +1,20 @@
function [Mu, Sigma] = productTPGMM(model, p)
-% Sylvain Calinon, Leonel Rozo, 2014
-%
-% Compute the product of Gaussians for a task-parametrized model where the
-% set of parameters are stored in the variable 'p'.
+%Compute the product of Gaussians for a task-parametrized model where the
+%set of parameters are stored in the variable 'p'.
+%Sylvain Calinon, 2015
-% TP-GMM products
-%See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
-for i = 1:model.nbStates
- % Reallocating
+%diagRegularizationFactor = 1E-6;
+diagRegularizationFactor = 1E-4;
+
+%TP-GMM products
+for i=1:model.nbStates
+ %Reallocating
SigmaTmp = zeros(model.nbVar);
MuTmp = zeros(model.nbVar,1);
- % Product of Gaussians
- for m = 1 : model.nbFrames
- MuP = p(m).A * model.Mu(:,m,i) + p(m).b;
- SigmaP = p(m).A * model.Sigma(:,:,m,i) * p(m).A';
+ %Product of Gaussians
+ for m=1:model.nbFrames
+ MuP = p(m).A * model.Mu(1:model.nbVars(m),m,i) + p(m).b;
+ SigmaP = p(m).A * model.Sigma(1:model.nbVars(m),1:model.nbVars(m),m,i) * p(m).A' + eye(model.nbVar)*diagRegularizationFactor;
SigmaTmp = SigmaTmp + inv(SigmaP);
MuTmp = MuTmp + SigmaP\MuP;
end
diff --git a/m_fcts/productTPGMM0.m b/m_fcts/productTPGMM0.m
new file mode 100644
index 0000000000000000000000000000000000000000..d2d13901f08e5fef3778d5a957f7ffd93cd13722
--- /dev/null
+++ b/m_fcts/productTPGMM0.m
@@ -0,0 +1,19 @@
+function [Mu, Sigma] = productTPGMM0(model, p)
+%Compute the product of Gaussians for a TP-GMM, where the set of task parameters are stored in the variable 'p'.
+%Sylvain Calinon, 2015
+
+%TP-GMM products, see Eqs (6.0.5)-(6.0.7) in doc/TechnicalReport.pdf
+for i=1:model.nbStates
+ % Reallocating
+ SigmaTmp = zeros(model.nbVar);
+ MuTmp = zeros(model.nbVar,1);
+ % Product of Gaussians
+ for m=1:model.nbFrames
+ MuP = p(m).A * model.Mu(:,m,i) + p(m).b;
+ SigmaP = p(m).A * model.Sigma(:,:,m,i) * p(m).A';
+ SigmaTmp = SigmaTmp + inv(SigmaP);
+ MuTmp = MuTmp + SigmaP\MuP;
+ end
+ Sigma(:,:,i) = inv(SigmaTmp);
+ Mu(:,i) = Sigma(:,:,i) * MuTmp;
+end
diff --git a/m_fcts/reproduction_DS.m b/m_fcts/reproduction_DS.m
index c6666b88bce2479e37f9db30e318e4daeb5ca57b..2446b19b9080456fe058565d9c9ddc27db801976 100644
--- a/m_fcts/reproduction_DS.m
+++ b/m_fcts/reproduction_DS.m
@@ -18,7 +18,7 @@ function r = reproduction_DS(DataIn, model, r, currPos)
% }
nbData = size(DataIn,2);
-nbVarOut = model.nbVar - size(DataIn,1);
+nbVarOut = length(currPos);
%% Reproduction with constant impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
diff --git a/m_fcts/reproduction_LQR_infiniteHorizon.m b/m_fcts/reproduction_LQR_infiniteHorizon.m
index 7059ee5c8b151891c96debdbc988940de9724bf0..2037c6a127d3bbd530001c1de7086799bd0a0059 100644
--- a/m_fcts/reproduction_LQR_infiniteHorizon.m
+++ b/m_fcts/reproduction_LQR_infiniteHorizon.m
@@ -52,13 +52,15 @@ for t=1:nbData
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%Alternatively, the function below can be used for an implementation based on Eigendecomposition
+ %-> the only operator is eig([A -B/R*B'; -Q -A'])
P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2); %See Eq. (5.1.27) and Sec. (5.2) in doc/TechnicalReport.pdf
%Variable for feedforward term computation (optional for movements with low dynamics)
- d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %See Eq. (5.1.28) in doc/TechnicalReport.pdf
+ d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %See Eq. (5.1.28) with d_dot=0 in doc/TechnicalReport.pdf
%Feedback term
L = R\B'*P; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
+
%Feedforward term
M = R\B'*d; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
diff --git a/m_fcts/reproduction_TPGMM.m b/m_fcts/reproduction_TPGMM.m
new file mode 100644
index 0000000000000000000000000000000000000000..3883636c573ec2b479dedbd8938ef338e35b30ff
--- /dev/null
+++ b/m_fcts/reproduction_TPGMM.m
@@ -0,0 +1,33 @@
+function r = reproduction_TPGMM(DataIn, model, r)
+% Estimation of an attractor path from a task-parameterized GMM and a set of candidate frames.
+%
+% Author: Sylvain Calinon, 2014
+% http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following reference in any academic publication that uses this code or part of it:
+%
+% @inproceedings{Calinon14ICRA,
+% 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"
+% }
+
+in = 1:size(DataIn,1);
+out = in(end)+1:model.nbVar;
+
+%% Estimation of the attractor path by Gaussian mixture regression,
+%% by using the GMM resulting from the product of linearly transformed Gaussians
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+rr.Priors = model.Priors;
+rr.nbStates = model.nbStates;
+for t=1:r.nbData
+ [rr.Mu, rr.Sigma] = productTPGMM(model, r.p(:,t));
+ [r.Data(:,t), r.Sigma(:,:,t)] = GMR(rr, DataIn(:,t), in, out);
+end
+
+
diff --git a/m_fcts/subaxis.m b/m_fcts/subaxis.m
new file mode 100644
index 0000000000000000000000000000000000000000..91aab0920885c8e9225bedf4b4cf9a0a85915b2a
--- /dev/null
+++ b/m_fcts/subaxis.m
@@ -0,0 +1,100 @@
+function h=subaxis(varargin)
+%SUBAXIS Create axes in tiled positions. (just like subplot)
+% Usage:
+% h=subaxis(rows,cols,cellno[,settings])
+% h=subaxis(rows,cols,cellx,celly[,settings])
+% h=subaxis(rows,cols,cellx,celly,spanx,spany[,settings])
+%
+% SETTINGS: Spacing,SpacingHoriz,SpacingVert
+% Padding,PaddingRight,PaddingLeft,PaddingTop,PaddingBottom
+% Margin,MarginRight,MarginLeft,MarginTop,MarginBottom
+% Holdaxis
+%
+% all units are relative (e.g from 0 to 1)
+%
+% Abbreviations of parameters can be used.. (Eg MR instead of MarginRight)
+% (holdaxis means that it wont delete any axes below.)
+%
+%
+% Example:
+%
+% >> subaxis(2,1,1,'SpacingVert',0,'MR',0);
+% >> imagesc(magic(3))
+% >> subaxis(2,'p',.02);
+% >> imagesc(magic(4))
+%
+% 2001 / Aslak Grinsted (Feel free to modify this code.)
+f=gcf;
+
+
+Args=[];
+UserDataArgsOK=0;
+Args=get(f,'UserData');
+if isstruct(Args)
+ UserDataArgsOK=isfield(Args,'SpacingHorizontal')&isfield(Args,'Holdaxis')&isfield(Args,'rows')&isfield(Args,'cols');
+end
+OKToStoreArgs=isempty(Args)|UserDataArgsOK;
+
+if isempty(Args) %&(~UserDataArgsOK)
+ Args=struct('Holdaxis',0, ...
+ 'SpacingVertical',0.05,'SpacingHorizontal',0.05, ...
+ 'PaddingLeft',0,'PaddingRight',0,'PaddingTop',0,'PaddingBottom',0, ...
+ 'MarginLeft',.1,'MarginRight',.1,'MarginTop',.1,'MarginBottom',.1, ...
+ 'rows',[],'cols',[]);
+end
+Args=parseArgs(varargin,Args,{'Holdaxis'},{'Spacing' {'sh','sv'}; 'Padding' {'pl','pr','pt','pb'}; 'Margin' {'ml','mr','mt','mb'}});
+
+if (length(Args.NumericArguments)>1)
+ Args.rows=Args.NumericArguments{1};
+ Args.cols=Args.NumericArguments{2};
+%remove these 2 numerical arguments
+ Args.NumericArguments={Args.NumericArguments{3:end}};
+end
+
+if OKToStoreArgs
+ set(f,'UserData',Args);
+end
+
+
+
+
+switch length(Args.NumericArguments)
+ case 0
+ return % no arguments but rows/cols....
+ case 1
+ x1=mod((Args.NumericArguments{1}-1),Args.cols)+1; x2=x1;
+ y1=floor((Args.NumericArguments{1}-1)/Args.cols)+1; y2=y1;
+ case 2
+ x1=Args.NumericArguments{1};x2=x1;
+ y1=Args.NumericArguments{2};y2=y1;
+ case 4
+ x1=Args.NumericArguments{1};x2=x1+Args.NumericArguments{3}-1;
+ y1=Args.NumericArguments{2};y2=y1+Args.NumericArguments{4}-1;
+ otherwise
+ error('subaxis argument error')
+end
+
+
+cellwidth=((1-Args.MarginLeft-Args.MarginRight)-(Args.cols-1)*Args.SpacingHorizontal)/Args.cols;
+cellheight=((1-Args.MarginTop-Args.MarginBottom)-(Args.rows-1)*Args.SpacingVertical)/Args.rows;
+xpos1=Args.MarginLeft+Args.PaddingLeft+cellwidth*(x1-1)+Args.SpacingHorizontal*(x1-1);
+xpos2=Args.MarginLeft-Args.PaddingRight+cellwidth*x2+Args.SpacingHorizontal*(x2-1);
+ypos1=Args.MarginTop+Args.PaddingTop+cellheight*(y1-1)+Args.SpacingVertical*(y1-1);
+ypos2=Args.MarginTop-Args.PaddingBottom+cellheight*y2+Args.SpacingVertical*(y2-1);
+
+if Args.Holdaxis
+ h=axes('position',[xpos1 1-ypos2 xpos2-xpos1 ypos2-ypos1]);
+else
+ h=subplot('position',[xpos1 1-ypos2 xpos2-xpos1 ypos2-ypos1]);
+end
+
+
+set(h,'box','on');
+%h=axes('position',[x1 1-y2 x2-x1 y2-y1]);
+set(h,'units',get(gcf,'defaultaxesunits'));
+set(h,'tag','subaxis');
+
+
+
+%if (nargout==0) clear h; end;
+