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;