diff --git a/README.md b/README.md
index bfded47dacf0482989c4f85b7c32e0fdf3601dad..086adb19c87c0faa957c6d7569d40b5af5acecda 100644
--- a/README.md
+++ b/README.md
@@ -6,8 +6,8 @@
### Usage
- Unzip the file and run 'demo01' in Matlab. Several reproduction algorithms can be selected by commenting/uncommenting
- lines 89-91 and 110-112 in demo01.m (finite/infinite horizon LQR or dynamical system with constant gains).
+ Unzip the file and run 'demo_TPGMR_LQR01' (finite horizon LQR), 'demo_TPGMR_LQR02' (infinite horizon LQR) or
+ 'demo_DSGMR01' (dynamical system with constant gains) in Matlab.
'demo_testLQR01', 'demo_testLQR02' and 'demo_testLQR03' can also be run as additional examples of LQR.
### Reference
diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..a426b9b4ac4c4e376202aa5d4cb120f031b2b270
--- /dev/null
+++ b/demo_DSGMR01.m
@@ -0,0 +1,193 @@
+function demo_DSGMR01
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
+% relevance of the frame through the task. This information is exploited to generate a new attractor path
+% corresponding to new situations (new positions and orientation of the frames).
+%
+% This demo presents the results for a dynamical system with constant gains.
+%
+% Author: Sylvain Calinon, 2014
+% http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following reference in any academic publication that uses this code or part of it:
+%
+% @inproceedings{Calinon14ICRA,
+% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+% title="A task-parameterized probabilistic model with minimal intervention control",
+% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+% year="2014",
+% month="May-June",
+% address="Hong Kong, China",
+% pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step
+model.kP = 100; %Stiffness gain (required only if LQR is not used for reproduction)
+model.kV = (2*model.kP)^.5; %Damping gain (required only if LQR is not used for reproduction)
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Transformation of 'Data' to learn the path of the spring-damper system
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbD = s(1).nbData;
+nbVarOut = model.nbVar - 1;
+%Create transformation matrix to compute [X; DX; DDX]
+D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
+D(end,end) = 0;
+%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
+K1d = [1, model.kV/model.kP, 1/model.kP];
+K = kron(K1d,eye(nbVarOut));
+%Create 3rd order tensor data with XHAT instead of X
+for n=1:nbSamples
+ DataTmp = s(n).Data0(2:end,:);
+ DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
+ for m=1:model.nbFrames
+ Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
+ end
+end
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+ %Retrieval of attractor path through task-parameterized GMR
+ a(n) = estimateAttractorPath(DataIn, model, s(n));
+ r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1));
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros
+ for m=1:model.nbFrames
+ %Random generation of new task parameters
+ id=ceil(rand(2,1)*nbSamples);
+ w=rand(2); w=w/sum(w);
+ rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+ rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+ end
+ %Retrieval of attractor path through task-parameterized GMR
+ anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+ rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1));
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+ %Plot frames
+ for m=1:model.nbFrames
+ plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+ plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+ end
+ %Plot trajectories
+ plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+ plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with DS-GMR');
+for n=1:nbSamples
+ %Plot frames
+ for m=1:model.nbFrames
+ plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+ plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+ end
+ %Plot Gaussians
+ plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+ %Plot trajectories
+ plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+ plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with DS-GMR');
+for n=1:nbRepros
+ %Plot frames
+ for m=1:model.nbFrames
+ plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+ plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+ end
+ %Plot Gaussians
+ plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+ %Plot trajectories
+ plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+ plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information
+figure;
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+ plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+ plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+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/demo_TPGMR_LQR01.m b/demo_TPGMR_LQR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..b1b5c3238db936ff2cbe9e28bdd687e2b8edb7a2
--- /dev/null
+++ b/demo_TPGMR_LQR01.m
@@ -0,0 +1,173 @@
+function demo_TPGMR_LQR01
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
+% relevance of the frame through the task. This information is exploited to generate a new attractor path
+% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances
+% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
+% the spring-damper systems, resulting in a minimal intervention control strategy.
+%
+% This demo presents the results for a finite horizon LQR.
+%
+% Author: Sylvain Calinon, 2014
+% http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following reference in any academic publication that uses this code or part of it:
+%
+% @inproceedings{Calinon14ICRA,
+% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+% title="A task-parameterized probabilistic model with minimal intervention control",
+% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+% year="2014",
+% month="May-June",
+% address="Hong Kong, China",
+% pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+ %Retrieval of attractor path through task-parameterized GMR
+ a(n) = estimateAttractorPath(DataIn, model, s(n));
+ r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros
+ for m=1:model.nbFrames
+ %Random generation of new task parameters
+ id=ceil(rand(2,1)*nbSamples);
+ w=rand(2); w=w/sum(w);
+ rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+ rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+ end
+ %Retrieval of attractor path through task-parameterized GMR
+ anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+ rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+ %Plot frames
+ for m=1:model.nbFrames
+ plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+ plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+ end
+ %Plot trajectories
+ plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+ plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with finite horizon LQR');
+for n=1:nbSamples
+ %Plot frames
+ for m=1:model.nbFrames
+ plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+ plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+ end
+ %Plot Gaussians
+ plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+ %Plot trajectories
+ plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+ plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with finite horizon LQR');
+for n=1:nbRepros
+ %Plot frames
+ for m=1:model.nbFrames
+ plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+ plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+ end
+ %Plot Gaussians
+ plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+ %Plot trajectories
+ plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+ plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information
+figure;
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+ plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+ plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+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/demo01.m b/demo_TPGMR_LQR02.m
similarity index 75%
rename from demo01.m
rename to demo_TPGMR_LQR02.m
index 701ccdf7118614d022dd394ad541ebfc7f7ae9a7..ff914e2b7513cbdecb0c17fc03b19ee021291d95 100644
--- a/demo01.m
+++ b/demo_TPGMR_LQR02.m
@@ -1,4 +1,4 @@
-function demo01
+function demo_TPGMR_LQR02
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of
% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
@@ -7,8 +7,7 @@ function demo01
% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
% the spring-damper systems, resulting in a minimal intervention control strategy.
%
-% Several reproduction algorithms can be selected by commenting/uncommenting lines 89-91 and 110-112
-% (finite/infinite horizon LQR or dynamical system with constant gains).
+% This demo presents the results for an infinite horizon LQR.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
@@ -48,28 +47,6 @@ disp('Load 3rd order tensor data...');
load('data/DataLQR01.mat');
-% %% Optional recomputation of 'Data' (only required when using reproduction_DS)
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% model.kP = 100; %Stiffness gain (required only if LQR is not used for reproduction)
-% model.kV = (2*model.kP)^.5; %Damping gain (required only if LQR is not used for reproduction)
-% nbD = s(1).nbData;
-% nbVarOut = model.nbVar - 1;
-% %Create transformation matrix to compute [X; DX; DDX]
-% D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
-% D(end,end) = 0;
-% %Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
-% K1d = [1, model.kV/model.kP, 1/model.kP];
-% K = kron(K1d,eye(nbVarOut));
-% %Create 3rd order tensor data with XHAT instead of X
-% for n=1:nbSamples
-% DataTmp = s(n).Data0(2:end,:);
-% DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
-% for m=1:model.nbFrames
-% Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
-% end
-% end
-
-
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of tensor GMM with EM:');
@@ -83,12 +60,8 @@ disp('Reproductions with LQR...');
DataIn = [1:s(1).nbData] * model.dt;
for n=1:nbSamples
%Retrieval of attractor path through task-parameterized GMR
- a(n) = estimateAttractorPath(DataIn, model, s(n));
-
- %Reproduction with one of the selected approach
- %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+ a(n) = estimateAttractorPath(DataIn, model, s(n));
r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
- %r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); %This function requires to define model.kP and model.kV (see lines 38-39)
end
@@ -105,11 +78,7 @@ for n=1:nbRepros
end
%Retrieval of attractor path through task-parameterized GMR
anew(n) = estimateAttractorPath(DataIn, model, rTmp);
-
- %Reproduction with one of the selected approach
- %rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
- %rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); %The fct requires to define model.kP and model.kV (see lines 38-39)
end
@@ -137,7 +106,7 @@ end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%REPROS
-subplot(1,3,2); hold on; box on; title('Reproductions with LQR');
+subplot(1,3,2); hold on; box on; title('Reproductions with infinite horizon LQR');
for n=1:nbSamples
%Plot frames
for m=1:model.nbFrames
@@ -155,7 +124,7 @@ end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%NEW REPROS
-subplot(1,3,3); hold on; box on; title('New reproductions with LQR');
+subplot(1,3,3); hold on; box on; title('New reproductions with infinite horizon LQR');
for n=1:nbRepros
%Plot frames
for m=1:model.nbFrames
@@ -172,7 +141,7 @@ for n=1:nbRepros
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-print('-dpng','outTest1.png');
+%print('-dpng','outTest1.png');
%Plot additional information
figure;
@@ -197,8 +166,7 @@ plot(r(n).FF(k,:),'b-','linewidth',2);
legend('ddx feedback','ddx feedforward');
xlabel('t'); ylabel(['ddx_' num2str(k)]);
-print('-dpng','outTest2.png');
-
+%print('-dpng','outTest2.png');
%pause;
%close all;