diff --git a/README.md b/README.md index 086adb19c87c0faa957c6d7569d40b5af5acecda..5d3faf7466e67f1c15c3a72fbe51eb19d8b476e2 100644 --- a/README.md +++ b/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 diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m index a426b9b4ac4c4e376202aa5d4cb120f031b2b270..2cc2509d34bc9be8bedb5d3e42c6e9a17993a82d 100644 --- a/demo_DSGMR01.m +++ b/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; diff --git a/estimateAttractorPath.m b/estimateAttractorPath.m index 7192bea70ca363fe83ccd39a38303faf315160ff..b1f4a3a237a8acb698c6d93d4901c8ea651a983b 100644 --- a/estimateAttractorPath.m +++ b/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 diff --git a/gaussianProduct.m b/productTPGMM.m similarity index 79% rename from gaussianProduct.m rename to productTPGMM.m index f30f24b9ae379a1c4d979e139aba9cac6a364b38..c58e505b32f412ad8a3d9b927097104e39455fe5 100644 --- a/gaussianProduct.m +++ b/productTPGMM.m @@ -1,12 +1,11 @@ -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 diff --git a/reproduction_DS.m b/reproduction_DS.m index 0ba79c3985482bc55aeb25853c5c67dcca90d933..3fb410f59cc393e62ada135f19beba0053a8da0f 100644 --- a/reproduction_DS.m +++ b/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