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 @@ ...@@ -2,7 +2,7 @@
### Compatibility ### Compatibility
The codes should be compatible with both Matlab and GNU Octave. The codes have been tested with both Matlab and GNU Octave.
### Usage ### Usage
......
...@@ -74,9 +74,9 @@ model = init_tensorGMM_timeBased(Data, model); %Initialization ...@@ -74,9 +74,9 @@ model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model); 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; DataIn = [1:s(1).nbData] * model.dt;
for n=1:nbSamples for n=1:nbSamples
%Retrieval of attractor path through task-parameterized GMR %Retrieval of attractor path through task-parameterized GMR
...@@ -85,9 +85,9 @@ for n=1:nbSamples ...@@ -85,9 +85,9 @@ for n=1:nbSamples
end 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 n=1:nbRepros
for m=1:model.nbFrames for m=1:model.nbFrames
%Random generation of new task parameters %Random generation of new task parameters
...@@ -178,15 +178,7 @@ for n=1:nbRepros ...@@ -178,15 +178,7 @@ for n=1:nbRepros
end end
xlabel('t'); ylabel('|Kp|'); 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; %pause;
%close all; %close all;
......
...@@ -17,53 +17,18 @@ function r = estimateAttractorPath(DataIn, model, r) ...@@ -17,53 +17,18 @@ function r = estimateAttractorPath(DataIn, model, r)
% pages="3339--3344" % pages="3339--3344"
% } % }
nbData = size(DataIn,2);
in = 1:size(DataIn,1); in = 1:size(DataIn,1);
out = in(end)+1:model.nbVar; 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 [r.Mu, r.Sigma] = productTPGMM(model, r.p);
for i=1:model.nbStates r.Priors = model.Priors;
SigmaTmp = zeros(model.nbVar); r.nbStates = model.nbStates;
MuTmp = zeros(model.nbVar,1);
for m=1:model.nbFrames [r.currTar, r.currSigma] = GMR(r, DataIn, in, out);
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
%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) function [Mu, Sigma] = productTPGMM(model, p)
% Sylvain Calinon, Leonel Rozo, 2014
% Leonel Rozo, 2014
% %
% Compute the product of Gaussians for a task-parametrized model where the % Compute the product of Gaussians for a task-parametrized model where the
% set of parameters are stored in the variable 'p'. % set of parameters are stored in the variable 'p'.
% GMM products % TP-GMM products
for i = 1 : model.nbStates for i = 1:model.nbStates
% Reallocating % Reallocating
SigmaTmp = zeros(model.nbVar); SigmaTmp = zeros(model.nbVar);
MuTmp = zeros(model.nbVar,1); MuTmp = zeros(model.nbVar,1);
...@@ -19,4 +18,4 @@ for i = 1 : model.nbStates ...@@ -19,4 +18,4 @@ for i = 1 : model.nbStates
end end
Sigma(:,:,i) = inv(SigmaTmp); Sigma(:,:,i) = inv(SigmaTmp);
Mu(:,i) = Sigma(:,:,i) * MuTmp; Mu(:,i) = Sigma(:,:,i) * MuTmp;
end end
\ No newline at end of file
...@@ -35,6 +35,7 @@ for t=1:nbData ...@@ -35,6 +35,7 @@ for t=1:nbData
r.Data(:,t) = [DataIn(:,t); x]; r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx); r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut)); r.kpDet(t) = det(L(:,1:nbVarOut));
r.kvDet(t) = det(L(:,nbVarOut+1:end));
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