Commit 789def57 by Milad Malekzadeh

### Tabs modified.

parent 0f3045f4
 function model = EM_tensorGMM(Data, model) % Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. % The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to % external parameters represented in the form of candidate coordinate systems. % Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. % The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to % external parameters represented in the form of candidate coordinate systems. % % 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: % 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.", ... ... @@ -34,131 +34,45 @@ for nbIter=1:nbMaxSteps [L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below and Eq. (2.0.5) in doc/TechnicalReport.pdf GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData); %M-step for i=1:model.nbStates for i=1:model.nbStates %Update Priors model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf for m=1:model.nbFrames %Matricization/flattening of tensor DataMat(:,:) = Data(:,m,:); %Update Mu %Update Mu model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf %Update Sigma (regularization term is optional) %Update Sigma (regularization term is optional) DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData); model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; %See Eq. (2.0.8) and (2.1.2) in doc/TechnicalReport.pdf end end %Compute average log-likelihood %Compute average log-likelihood LL(nbIter) = sum(log(sum(L,1))) / size(L,2); %See Eq. (2.0.4) in doc/TechnicalReport.pdf %Stop the algorithm if EM converged (small change of LL) if nbIter>nbMinSteps if LL(nbIter)-LL(nbIter-1)nbMinSteps if LL(nbIter)-LL(nbIter-1)
 ... ... @@ -12,7 +12,7 @@ for t=1:nbData %Compute activation weight %See Eq. (3.0.5) in doc/TechnicalReport.pdf for i=1:model.nbStates H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i)); H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i)); end H(:,t) = H(:,t)/sum(H(:,t)); %Compute expected conditional means ... ... @@ -25,9 +25,9 @@ for t=1:nbData %See Eq. (3.0.4) in doc/TechnicalReport.pdf for i=1:model.nbStates SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i); expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)'); expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)'); for j=1:model.nbStates expSigma(:,:,t) = expSigma(:,:,t) - H(i,t)*H(j,t) * (MuTmp(:,i)*MuTmp(:,j)'); expSigma(:,:,t) = expSigma(:,:,t) - H(i,t)*H(j,t) * (MuTmp(:,i)*MuTmp(:,j)'); end end end ... ...
 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 % 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: % 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.", ... ... @@ -30,7 +30,7 @@ function demo_TPGMR_LQR01 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.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 ... ... @@ -38,10 +38,10 @@ 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 % 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'); ... ... @@ -50,7 +50,7 @@ load('data/DataLQR01.mat'); %% Tensor GMM learning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fprintf('Parameters estimation of tensor GMM with EM:'); model = init_tensorGMM_timeBased(Data, model); %Initialization model = init_tensorGMM_timeBased(Data, model); %Initialization model = EM_tensorGMM(Data, model); ... ... @@ -68,9 +68,9 @@ end %% Reproduction with LQR for new task parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('New reproductions with LQR...'); for n=1:nbRepros for n=1:nbRepros for m=1:model.nbFrames %Random generation of new task parameters %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); ... ... @@ -143,8 +143,8 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); %print('-dpng','outTest1.png'); %Plot additional information figure; %Plot additional information figure; %Plot norm of control commands subplot(1,2,1); hold on; for n=1:nbRepros ... ... @@ -169,178 +169,3 @@ xlabel('t'); ylabel(['ddx_' num2str(k)]); %print('-dpng','outTest2.png'); %pause; %close all; 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;
 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 % 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 % 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 an infinite 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: % 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.", ... ... @@ -30,7 +30,7 @@ function demo_TPGMR_LQR02 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.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 ... ... @@ -38,10 +38,10 @@ 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 % 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'); ... ... @@ -50,7 +50,7 @@ load('data/DataLQR01.mat'); %% Tensor GMM learning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fprintf('Parameters estimation of tensor GMM with EM:'); model = init_tensorGMM_timeBased(Data, model); %Initialization model = init_tensorGMM_timeBased(Data, model); %Initialization model = EM_tensorGMM(Data, model); ... ... @@ -60,7 +60,7 @@ 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)); a(n) = estimateAttractorPath(DataIn, model, s(n)); r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor); end ... ... @@ -68,9 +68,9 @@ end %% Reproduction with LQR for new task parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% disp('New reproductions with LQR...'); for n=1:nbRepros for n=1:nbRepros for m=1:model.nbFrames %Random generation of new task parameters %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); ... ... @@ -143,8 +143,8 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); %print('-dpng','outTest1.png'); %Plot additional information figure; %Plot additional information figure; %Plot norm of control commands subplot(1,2,1); hold on; for n=1:nbRepros ... ... @@ -169,178 +169,3 @@ xlabel('t'); ylabel(['ddx_' num2str(k)]); %print('-dpng','outTest2.png'); %pause; %close all; 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 % 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 an infinite 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