Skip to content
Snippets Groups Projects
Commit cb57746d authored by Sylvain Calinon's avatar Sylvain Calinon
Browse files

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

parent 191b8988
No related branches found
No related tags found
No related merge requests found
...@@ -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);
......
...@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment