Commit cb57746d authored by Sylvain Calinon's avatar Sylvain Calinon

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

parent 191b8988
......@@ -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
......
......@@ -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;
......
......@@ -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
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
......@@ -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
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment