Split of the estimateAttractorPath function into productTPGMM.m and GMR.m

README.md

@@ -2,7 +2,7 @@
 
 ### Compatibility
 
-	The codes should be compatible with both Matlab and GNU Octave.
+	The codes have been tested with both Matlab and GNU Octave.
 
 ### Usage
 

demo_DSGMR01.m

@@ -74,9 +74,9 @@ model = init_tensorGMM_timeBased(Data, model); %Initialization
 model = EM_tensorGMM(Data, model);
 
 
-%% Reproduction with LQR for the task parameters used to train the model
+%% Reproduction with DS-GMR for the task parameters used to train the model
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with LQR...');
+disp('Reproductions with DS-GMR...');
 DataIn = [1:s(1).nbData] * model.dt;
 for n=1:nbSamples
   %Retrieval of attractor path through task-parameterized GMR
@@ -85,9 +85,9 @@ for n=1:nbSamples
 end
 
 
-%% Reproduction with LQR for new task parameters
+%% Reproduction with DS-GMR for new task parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('New reproductions with LQR...');
+disp('New reproductions with DS-GMR...');
 for n=1:nbRepros 
   for m=1:model.nbFrames
     %Random generation of new task parameters 
@@ -178,15 +178,7 @@ for n=1:nbRepros
 end
 xlabel('t'); ylabel('|Kp|');
 
-%Plot accelerations due to feedback and feedforward terms
-figure; hold on;
-n=1; k=1;
-plot(r(n).FB(k,:),'r-','linewidth',2);
-plot(r(n).FF(k,:),'b-','linewidth',2);
-legend('ddx feedback','ddx feedforward');
-xlabel('t'); ylabel(['ddx_' num2str(k)]);
 
-%print('-dpng','outTest2.png');
 %pause;
 %close all;
 

estimateAttractorPath.m

@@ -17,53 +17,18 @@ function r = estimateAttractorPath(DataIn, model, r)
 %   pages="3339--3344"
 % }
 
-nbData = size(DataIn,2);
 in = 1:size(DataIn,1);
 out = in(end)+1:model.nbVar;
-nbVarOut = length(out);
 
-%% GMR to estimate attractor path and associated variations
+%% Estimation of the attractor path by Gaussian mixture regression,
+%% by using the GMM resulting from the product of linearly transformed Gaussians
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%GMM products 
-for i=1:model.nbStates 
-  SigmaTmp = zeros(model.nbVar);
-  MuTmp = zeros(model.nbVar,1);
-  for m=1:model.nbFrames 
-    MuP = r.p(m).A * model.Mu(:,m,i) + r.p(m).b; 
-    SigmaP = r.p(m).A * model.Sigma(:,:,m,i) * r.p(m).A'; 
-    SigmaTmp = SigmaTmp + inv(SigmaP);
-    MuTmp = MuTmp + SigmaP\MuP; 
-  end
-  r.Sigma(:,:,i) = inv(SigmaTmp);
-  r.Mu(:,i) = r.Sigma(:,:,i) * MuTmp;
-end
+[r.Mu, r.Sigma] = productTPGMM(model, r.p);
+r.Priors = model.Priors;  
+r.nbStates = model.nbStates;
+
+[r.currTar, r.currSigma] = GMR(r, DataIn, in, out);
+
 
-%GMR
-MuTmp = zeros(nbVarOut,model.nbStates);
-for t=1:nbData
-  %Compute activation weight
-  for i=1:model.nbStates
-    r.H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), r.Mu(in,i), r.Sigma(in,in,i)); 
-  end
-  r.H(:,t) = r.H(:,t)/sum(r.H(:,t));
-  %Evaluate the current target 
-  currTar = zeros(nbVarOut,1);
-  currSigma = zeros(nbVarOut,nbVarOut);   
-  %Compute expected conditional means
-	for i=1:model.nbStates
-		MuTmp(:,i) = r.Mu(out,i) + r.Sigma(out,in,i)/r.Sigma(in,in,i) * (DataIn(:,t)-r.Mu(in,i));
-		currTar = currTar + r.H(i,t) * MuTmp(:,i);
-	end
-	%Compute expected conditional covariances
-	for i=1:model.nbStates
-		SigmaTmp = r.Sigma(out,out,i) - r.Sigma(out,in,i)/r.Sigma(in,in,i) * r.Sigma(in,out,i);
-		currSigma = currSigma + r.H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)');  
-	 	for j=1:model.nbStates
-		 	currSigma = currSigma - r.H(i,t)*r.H(j,t) * (MuTmp(:,i)*MuTmp(:,j)');  
-	 	end
-	end
-  r.currTar(:,t) = currTar; 
-  r.currSigma(:,:,t) = currSigma;
-end
 

gaussianProduct.m → productTPGMM.m

-function [Mu, Sigma] = gaussianProduct(model, p)
-
-% Leonel Rozo, 2014
+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'. 
 
-% GMM products 
-for i = 1 : model.nbStates
+% TP-GMM products 
+for i = 1:model.nbStates
   % Reallocating  
   SigmaTmp = zeros(model.nbVar);
   MuTmp = zeros(model.nbVar,1);
@@ -19,4 +18,4 @@ for i = 1 : model.nbStates
   end
   Sigma(:,:,i) = inv(SigmaTmp);
   Mu(:,i) = Sigma(:,:,i) * MuTmp;    
-end
\ No newline at end of file
+end

reproduction_DS.m

@@ -35,6 +35,7 @@ for t=1:nbData
   r.Data(:,t) = [DataIn(:,t); x]; 
   r.ddxNorm(t) = norm(ddx);
   r.kpDet(t) = det(L(:,1:nbVarOut));
+  r.kvDet(t) = det(L(:,nbVarOut+1:end));
 end