diff --git a/EM_tensorGMM.m b/EM_tensorGMM.m index b4232e30fad02cc71cae56e45c811fba2411c6e9..4f60aae8c1857231d0b00b3864dded086fedb90b 100644 --- a/EM_tensorGMM.m +++ b/EM_tensorGMM.m @@ -1,3 +1,85 @@ +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. +% +% 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 of the EM algorithm +nbMinSteps = 5; %Minimum number of iterations allowed +nbMaxSteps = 100; %Maximum number of iterations allowed +maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm +nbData = size(Data,3); + +%diagRegularizationFactor = 1E-2; +diagRegularizationFactor = 1E-4; + +for nbIter=1:nbMaxSteps + fprintf('.'); + %E-step + [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 + %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 + model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf + %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 + 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)<maxDiffLL || nbIter==nbMaxSteps-1 + disp(['EM converged after ' num2str(nbIter) ' iterations.']); + return; + end + end +end +disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']); +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function [L, GAMMA, GAMMA0] = computeGamma(Data, model) + %See Eq. (2.0.5) in doc/TechnicalReport.pdf + nbData = size(Data, 3); + L = ones(model.nbStates, nbData); + GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData); + for m=1:model.nbFrames + DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor + for i=1:model.nbStates + GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i)); + L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))'; + end + end + %Normalization + GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1); +end + + + + 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 diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m index f136b698065770746d2a77d74e621453eff4ac57..6630adb2996318b6efc7a683ed6107e771c4dcc9 100644 --- a/demo_DSGMR01.m +++ b/demo_DSGMR01.m @@ -1,183 +1,183 @@ -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 -model.kV = (2*model.kP)^.5; %Damping gain -nbRepros = 8; %Number of reproductions with new situations randomly generated - -%% 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 DS-GMR for the task parameters used to train the model -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('Reproductions with DS-GMR...'); -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 DS-GMR for new task parameters -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('New reproductions with DS-GMR...'); -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|'); - - -%pause; -%close all; - - +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 +model.kV = (2*model.kP)^.5; %Damping gain +nbRepros = 8; %Number of reproductions with new situations randomly generated + +%% 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 DS-GMR for the task parameters used to train the model +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with DS-GMR...'); +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 DS-GMR for new task parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('New reproductions with DS-GMR...'); +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|'); + + +%pause; +%close all; + + diff --git a/demo_TPGMR_LQR01.m b/demo_TPGMR_LQR01.m index b1b5c3238db936ff2cbe9e28bdd687e2b8edb7a2..78d1e8aa3d6a613d2754c5d59ef8ae53ad08946a 100644 --- a/demo_TPGMR_LQR01.m +++ b/demo_TPGMR_LQR01.m @@ -1,173 +1,346 @@ -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_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_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/demo_TPGMR_LQR02.m b/demo_TPGMR_LQR02.m index ff914e2b7513cbdecb0c17fc03b19ee021291d95..2b77de7a448dee84330d1b290528c25f2c32d1f3 100644 --- a/demo_TPGMR_LQR02.m +++ b/demo_TPGMR_LQR02.m @@ -1,173 +1,346 @@ -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 -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -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_infiniteHorizon(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_infiniteHorizon(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 infinite 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 infinite 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 +% 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 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +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_infiniteHorizon(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_infiniteHorizon(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 infinite 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 infinite 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 +% 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 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +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_infiniteHorizon(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_infiniteHorizon(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 infinite 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 infinite 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/demo_testLQR01.m b/demo_testLQR01.m index 5a9283211b983c44367c3c674bdb02d12520c62e..7e0290ad3e3bf281a3cc0b1f46fb2b7c3e09d262 100644 --- a/demo_testLQR01.m +++ b/demo_testLQR01.m @@ -1,66 +1,132 @@ -function demo_testLQR01 -% Test of the linear quadratic regulation -% -% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) -model.dt = 0.01; %Time step -nbData = 500; %Number of datapoints -nbRepros = 3; %Number of reproductions with new situations randomly generated -rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR - -%% Reproduction with LQR -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('Reproductions with LQR...'); -DataIn = [1:nbData] * model.dt; -a.currTar = ones(1,nbData); -for n=1:nbRepros - a.currSigma = ones(1,1,nbData) * 10^(2-n); - %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); - r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); -end - -%% Plots -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -figure('position',[20,50,1300,500]); -hold on; box on; -%Plot target -plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); -for n=1:nbRepros - %Plot trajectories - plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('x_1'); - -figure; -%Plot norm of control commands -subplot(1,2,1); hold on; -for n=1:nbRepros - plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('|ddx|'); -%Plot stiffness -subplot(1,2,2); hold on; -for n=1:nbRepros - plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('kp'); - -%pause; -%close all; +function demo_testLQR01 +% Test of the linear quadratic regulation +% +% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) +model.dt = 0.01; %Time step +nbData = 500; %Number of datapoints +nbRepros = 3; %Number of reproductions with new situations randomly generated +rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR + +%% Reproduction with LQR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with LQR...'); +DataIn = [1:nbData] * model.dt; +a.currTar = ones(1,nbData); +for n=1:nbRepros + a.currSigma = ones(1,1,nbData) * 10^(2-n); + %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); +end + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[20,50,1300,500]); +hold on; box on; +%Plot target +plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); +for n=1:nbRepros + %Plot trajectories + plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('x_1'); + +figure; +%Plot norm of control commands +subplot(1,2,1); hold on; +for n=1:nbRepros + plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('|ddx|'); +%Plot stiffness +subplot(1,2,2); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('kp'); + +%pause; +%close all; +function demo_testLQR01 +% Test of the linear quadratic regulation +% +% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) +model.dt = 0.01; %Time step +nbData = 500; %Number of datapoints +nbRepros = 3; %Number of reproductions with new situations randomly generated +rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR + +%% Reproduction with LQR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with LQR...'); +DataIn = [1:nbData] * model.dt; +a.currTar = ones(1,nbData); +for n=1:nbRepros + a.currSigma = ones(1,1,nbData) * 10^(2-n); + %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); +end + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[20,50,1300,500]); +hold on; box on; +%Plot target +plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); +for n=1:nbRepros + %Plot trajectories + plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('x_1'); + +figure; +%Plot norm of control commands +subplot(1,2,1); hold on; +for n=1:nbRepros + plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('|ddx|'); +%Plot stiffness +subplot(1,2,2); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('kp'); + +%pause; +%close all; diff --git a/demo_testLQR02.m b/demo_testLQR02.m index 9d4489f7df49d0c213e682819e9855012a6e8aab..f7a765152b4d8df95efa631faa2bf44c6b340d70 100644 --- a/demo_testLQR02.m +++ b/demo_testLQR02.m @@ -1,74 +1,148 @@ -function demo_testLQR02 -% Test of the linear quadratic regulation -% -% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) -model.dt = 0.01; %Time step -nbData = 1000; %Number of datapoints -nbRepros = 1; %Number of reproductions with new situations randomly generated -rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR - -%% Reproduction with LQR -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('Reproductions with LQR...'); -DataIn = [1:nbData] * model.dt; -a.currTar = ones(1,nbData); -a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u -for n=1:nbRepros - %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); - r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); -end - -%% Plots -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -figure('position',[20,50,1300,500]); -hold on; box on; -%Plot target -plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); -for n=1:nbRepros - %Plot trajectories - plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2); -end -xlabel('t'); ylabel('x_1'); - -figure; -%Plot norm of control commands -subplot(1,3,1); hold on; -for n=1:nbRepros - plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2); -end -xlabel('t'); ylabel('|ddx|'); -%Plot stiffness -subplot(1,3,2); hold on; -for n=1:nbRepros - plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2); -end -xlabel('t'); ylabel('kp'); -%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5) -subplot(1,3,3); hold on; -for n=1:nbRepros - plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2); -end -xlabel('t'); ylabel('kp/kv'); - -r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071 - -%pause; -%close all; +function demo_testLQR02 +% Test of the linear quadratic regulation +% +% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) +model.dt = 0.01; %Time step +nbData = 1000; %Number of datapoints +nbRepros = 1; %Number of reproductions with new situations randomly generated +rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR + +%% Reproduction with LQR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with LQR...'); +DataIn = [1:nbData] * model.dt; +a.currTar = ones(1,nbData); +a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u +for n=1:nbRepros + %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); +end + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[20,50,1300,500]); +hold on; box on; +%Plot target +plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); +for n=1:nbRepros + %Plot trajectories + plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('x_1'); + +figure; +%Plot norm of control commands +subplot(1,3,1); hold on; +for n=1:nbRepros + plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('|ddx|'); +%Plot stiffness +subplot(1,3,2); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('kp'); +%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5) +subplot(1,3,3); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('kp/kv'); + +r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071 + +%pause; +%close all; +function demo_testLQR02 +% Test of the linear quadratic regulation +% +% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) +model.dt = 0.01; %Time step +nbData = 1000; %Number of datapoints +nbRepros = 1; %Number of reproductions with new situations randomly generated +rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR + +%% Reproduction with LQR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with LQR...'); +DataIn = [1:nbData] * model.dt; +a.currTar = ones(1,nbData); +a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u +for n=1:nbRepros + %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); + r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); +end + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[20,50,1300,500]); +hold on; box on; +%Plot target +plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); +for n=1:nbRepros + %Plot trajectories + plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('x_1'); + +figure; +%Plot norm of control commands +subplot(1,3,1); hold on; +for n=1:nbRepros + plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('|ddx|'); +%Plot stiffness +subplot(1,3,2); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('kp'); +%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5) +subplot(1,3,3); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2); +end +xlabel('t'); ylabel('kp/kv'); + +r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071 + +%pause; +%close all; diff --git a/demo_testLQR03.m b/demo_testLQR03.m index 97bcbe1e1cf6236d427d46d0ad8604eb8bdd9223..d1c51f729d28cdc38321249de3690c0a6d95f68f 100644 --- a/demo_testLQR03.m +++ b/demo_testLQR03.m @@ -1,99 +1,198 @@ -function demo_testLQR03 -% Comaprison of linear quadratic regulators with finite and infinite time horizons -% -% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) -model.dt = 0.01; %Time step -nbData = 400; %Number of datapoints -nbRepros = 2; %Number of reproductions with new situations randomly generated -rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR - -%% Reproduction with LQR -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -disp('Reproductions with LQR...'); -DataIn = [1:nbData] * model.dt; -%a.currTar = ones(1,nbData); -a.currTar = linspace(100,100,nbData); -%a.currTar = sin(linspace(0,8*pi,nbData)); - -%a.currSigma = ones(1,1,nbData); -a.currSigma = ones(1,1,nbData-100) * 100; -a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1; -aFinal.currTar = a.currTar(:,end); -aFinal.currSigma = a.currSigma(:,:,end); - -for n=1:nbRepros - if n==1 - r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); - else - %First call to LQR to get an estimate of the final feedback terms - [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor); - %Second call to LQR with finite horizon - r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal); - end -end -for n=1:nbRepros - %Evaluation of determinant (for analysis purpose) - for t=1:nbData - r(n).detSigma(t) = det(a.currSigma(:,:,t)); - end -end - -%% Plots -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -figure('position',[20,50,1300,500]); -hold on; box on; -%Plot target -plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); -for n=1:nbRepros - %Plot trajectories - plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('x_1'); - -figure; -%Plot variations -subplot(2,3,[1,4]); hold on; -for n=1:nbRepros - plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('|\Sigma|'); -%Plot norm of control commands -subplot(2,3,[2,5]); hold on; -for n=1:nbRepros - plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('|ddx|'); -%Plot stiffness -subplot(2,3,3); hold on; -for n=1:nbRepros - plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('kp'); -%Plot damping -subplot(2,3,6); hold on; -for n=1:nbRepros - plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); -end -xlabel('t'); ylabel('kv'); - -%pause; -%close all; - +function demo_testLQR03 +% Comaprison of linear quadratic regulators with finite and infinite time horizons +% +% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) +model.dt = 0.01; %Time step +nbData = 400; %Number of datapoints +nbRepros = 2; %Number of reproductions with new situations randomly generated +rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR + +%% Reproduction with LQR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with LQR...'); +DataIn = [1:nbData] * model.dt; +%a.currTar = ones(1,nbData); +a.currTar = linspace(100,100,nbData); +%a.currTar = sin(linspace(0,8*pi,nbData)); + +%a.currSigma = ones(1,1,nbData); +a.currSigma = ones(1,1,nbData-100) * 100; +a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1; +aFinal.currTar = a.currTar(:,end); +aFinal.currSigma = a.currSigma(:,:,end); + +for n=1:nbRepros + if n==1 + r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); + else + %First call to LQR to get an estimate of the final feedback terms + [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor); + %Second call to LQR with finite horizon + r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal); + end +end +for n=1:nbRepros + %Evaluation of determinant (for analysis purpose) + for t=1:nbData + r(n).detSigma(t) = det(a.currSigma(:,:,t)); + end +end + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[20,50,1300,500]); +hold on; box on; +%Plot target +plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); +for n=1:nbRepros + %Plot trajectories + plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('x_1'); + +figure; +%Plot variations +subplot(2,3,[1,4]); hold on; +for n=1:nbRepros + plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('|\Sigma|'); +%Plot norm of control commands +subplot(2,3,[2,5]); hold on; +for n=1:nbRepros + plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('|ddx|'); +%Plot stiffness +subplot(2,3,3); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('kp'); +%Plot damping +subplot(2,3,6); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('kv'); + +%pause; +%close all; + +function demo_testLQR03 +% Comaprison of linear quadratic regulators with finite and infinite time horizons +% +% 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.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) +model.dt = 0.01; %Time step +nbData = 400; %Number of datapoints +nbRepros = 2; %Number of reproductions with new situations randomly generated +rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR + +%% Reproduction with LQR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Reproductions with LQR...'); +DataIn = [1:nbData] * model.dt; +%a.currTar = ones(1,nbData); +a.currTar = linspace(100,100,nbData); +%a.currTar = sin(linspace(0,8*pi,nbData)); + +%a.currSigma = ones(1,1,nbData); +a.currSigma = ones(1,1,nbData-100) * 100; +a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1; +aFinal.currTar = a.currTar(:,end); +aFinal.currSigma = a.currSigma(:,:,end); + +for n=1:nbRepros + if n==1 + r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); + else + %First call to LQR to get an estimate of the final feedback terms + [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor); + %Second call to LQR with finite horizon + r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal); + end +end +for n=1:nbRepros + %Evaluation of determinant (for analysis purpose) + for t=1:nbData + r(n).detSigma(t) = det(a.currSigma(:,:,t)); + end +end + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[20,50,1300,500]); +hold on; box on; +%Plot target +plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); +for n=1:nbRepros + %Plot trajectories + plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('x_1'); + +figure; +%Plot variations +subplot(2,3,[1,4]); hold on; +for n=1:nbRepros + plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('|\Sigma|'); +%Plot norm of control commands +subplot(2,3,[2,5]); hold on; +for n=1:nbRepros + plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('|ddx|'); +%Plot stiffness +subplot(2,3,3); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('kp'); +%Plot damping +subplot(2,3,6); hold on; +for n=1:nbRepros + plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); +end +xlabel('t'); ylabel('kv'); + +%pause; +%close all; + diff --git a/estimateAttractorPath.m b/estimateAttractorPath.m index 62653ded260d212ac31ae2a58b510be2d39a8821..ca5d3a4f85e9da4c2a0269108ac2ab971bd63727 100644 --- a/estimateAttractorPath.m +++ b/estimateAttractorPath.m @@ -32,3 +32,37 @@ r.nbStates = model.nbStates; +function r = estimateAttractorPath(DataIn, model, r) +% Estimation of an attractor path from a task-parameterized GMM and a set of candidate frames. +% +% 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" +% } + +in = 1:size(DataIn,1); +out = in(end)+1:model.nbVar; + +%% Estimation of the attractor path by Gaussian mixture regression, +%% by using the GMM resulting from the product of linearly transformed Gaussians +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +[r.Mu, r.Sigma] = productTPGMM(model, r.p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf +r.Priors = model.Priors; +r.nbStates = model.nbStates; + +[r.currTar, r.currSigma] = GMR(r, DataIn, in, out); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf + + + diff --git a/gaussPDF.m b/gaussPDF.m index f53aee739d83f18606dbbc0cb5c55b6093642ed1..326a26bb37433ee6eccd46adfde5abfedf4fcbc2 100644 --- a/gaussPDF.m +++ b/gaussPDF.m @@ -16,3 +16,21 @@ function prob = gaussPDF(Data, Mu, Sigma) Data = Data' - repmat(Mu',nbData,1); prob = sum((Data/Sigma).*Data, 2); prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin)); +function prob = gaussPDF(Data, Mu, Sigma) +% Likelihood of datapoint(s) to be generated by a Gaussian parameterized by center and covariance. +% +% Inputs ----------------------------------------------------------------- +% o Data: D x N array representing N datapoints of D dimensions. +% o Mu: D x 1 vector representing the center of the Gaussian. +% o Sigma: D x D array representing the covariance matrix of the Gaussian. +% Output ----------------------------------------------------------------- +% o prob: 1 x N vector representing the likelihood of the N datapoints. +% +% Author: Sylvain Calinon, 2014 +% http://programming-by-demonstration.org/SylvainCalinon + +[nbVar,nbData] = size(Data); +%See Eq. (2.0.3) in doc/TechnicalReport.pdf +Data = Data' - repmat(Mu',nbData,1); +prob = sum((Data/Sigma).*Data, 2); +prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin)); diff --git a/init_tensorGMM_kmeans.m b/init_tensorGMM_kmeans.m index 3fc3eaa84e3a70705a73bf4bad02795268063a01..a0089e69a260744e1583b9e9928808f44f3aae02 100644 --- a/init_tensorGMM_kmeans.m +++ b/init_tensorGMM_kmeans.m @@ -11,18 +11,18 @@ DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization [Data_id, Mu] = kmeansClustering(DataAll, model.nbStates); for i=1:model.nbStates - idtmp = find(Data_id==i); - model.Priors(i) = length(idtmp); - Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor; + idtmp = find(Data_id==i); + model.Priors(i) = length(idtmp); + Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor; end model.Priors = model.Priors / sum(model.Priors); %Reshape GMM parameters into a tensor for m=1:model.nbFrames - for i=1:model.nbStates - model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); - model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); - end + for i=1:model.nbStates + model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); + model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); + end end diff --git a/init_tensorGMM_timeBased.m b/init_tensorGMM_timeBased.m index 010c80ba64a3fe4f1e50740ef5eeeb1de1bb3642..fdfe089f8afb5e95fb1f36c6a90b62550db82b96 100644 --- a/init_tensorGMM_timeBased.m +++ b/init_tensorGMM_timeBased.m @@ -1,27 +1,27 @@ -function model = init_tensorGMM_timeBased(Data, model) -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon - -diagRegularizationFactor = 1E-4; - -DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor - -TimingSep = linspace(min(DataAll(1,:)), max(DataAll(1,:)), model.nbStates+1); -Mu = zeros(model.nbFrames*model.nbVar, model.nbStates); -Sigma = zeros(model.nbFrames*model.nbVar, model.nbFrames*model.nbVar, model.nbStates); -for i=1:model.nbStates - idtmp = find( DataAll(1,:)>=TimingSep(i) & DataAll(1,:)<TimingSep(i+1)); - Mu(:,i) = mean(DataAll(:,idtmp),2); - Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor; - model.Priors(i) = length(idtmp); -end -model.Priors = model.Priors / sum(model.Priors); - -%Reshape GMM parameters into a tensor -for m=1:model.nbFrames - for i=1:model.nbStates - model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); - model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); - end -end - +function model = init_tensorGMM_timeBased(Data, model) +% Author: Sylvain Calinon, 2014 +% http://programming-by-demonstration.org/SylvainCalinon + +diagRegularizationFactor = 1E-4; + +DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor + +TimingSep = linspace(min(DataAll(1,:)), max(DataAll(1,:)), model.nbStates+1); +Mu = zeros(model.nbFrames*model.nbVar, model.nbStates); +Sigma = zeros(model.nbFrames*model.nbVar, model.nbFrames*model.nbVar, model.nbStates); +for i=1:model.nbStates + idtmp = find( DataAll(1,:)>=TimingSep(i) & DataAll(1,:)<TimingSep(i+1)); + Mu(:,i) = mean(DataAll(:,idtmp),2); + Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor; + model.Priors(i) = length(idtmp); +end +model.Priors = model.Priors / sum(model.Priors); + +%Reshape GMM parameters into a tensor +for m=1:model.nbFrames + for i=1:model.nbStates + model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); + model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); + end +end + diff --git a/kmeansClustering.m b/kmeansClustering.m index c5e5696b9f8287279c9a044b9be5e8b5d75f15c9..894585fb7d6ec702e6117a46c61f31b8e067ed45 100644 --- a/kmeansClustering.m +++ b/kmeansClustering.m @@ -17,24 +17,24 @@ Mu = Data(:,idTmp(1:nbStates)); %k-means iterations while 1 - %E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - for i=1:nbStates - %Compute distances - distTmp(:,i) = sum((Data-repmat(Mu(:,i),1,nbData)).^2); - end - [vTmp,idList] = min(distTmp,[],2); - cumdist = sum(vTmp); - %M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - for i=1:nbStates - %Update the centers - Mu(:,i) = mean(Data(:,idList==i),2); - end - %Stopping criterion %%%%%%%%%%%%%%%%%%%% - if abs(cumdist-cumdist_old) < cumdist_threshold - break; - end - cumdist_old = cumdist; - nbStep = nbStep+1; + %E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for i=1:nbStates + %Compute distances + distTmp(:,i) = sum((Data-repmat(Mu(:,i),1,nbData)).^2); + end + [vTmp,idList] = min(distTmp,[],2); + cumdist = sum(vTmp); + %M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for i=1:nbStates + %Update the centers + Mu(:,i) = mean(Data(:,idList==i),2); + end + %Stopping criterion %%%%%%%%%%%%%%%%%%%% + if abs(cumdist-cumdist_old) < cumdist_threshold + break; + end + cumdist_old = cumdist; + nbStep = nbStep+1; % if nbStep>maxIter % disp(['Maximum number of iterations, ' num2str(maxIter) 'is reached']); % break; diff --git a/plotGMM.m b/plotGMM.m index e419dc67aa2bd3850bad519a993fff0f13b1cb3d..14e144ba106f547c1f106335305aabbddcd959ad 100644 --- a/plotGMM.m +++ b/plotGMM.m @@ -1,22 +1,22 @@ -function h = plotGMM(Mu, Sigma, color) -% This function displays the parameters of a Gaussian Mixture Model (GMM). -% Inputs ----------------------------------------------------------------- -% o Mu: D x K array representing the centers of K Gaussians. -% o Sigma: D x D x K array representing the covariance matrices of K Gaussians. -% o color: 3 x 1 array representing the RGB color to use for the display. -% -% Author: Sylvain Calinon, 2014 -% http://programming-by-demonstration.org/SylvainCalinon - -nbStates = size(Mu,2); -nbDrawingSeg = 35; -lightcolor = min(color+0.3,1); -t = linspace(-pi, pi, nbDrawingSeg); - -h=[]; -for i=1:nbStates - R = real(sqrtm(1.0.*Sigma(:,:,i))); - X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg); - h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)]; - h = [h plot(Mu(1,:), Mu(2,:), 'x', 'lineWidth', 2, 'markersize', 6, 'color', color)]; -end +function h = plotGMM(Mu, Sigma, color) +% This function displays the parameters of a Gaussian Mixture Model (GMM). +% Inputs ----------------------------------------------------------------- +% o Mu: D x K array representing the centers of K Gaussians. +% o Sigma: D x D x K array representing the covariance matrices of K Gaussians. +% o color: 3 x 1 array representing the RGB color to use for the display. +% +% Author: Sylvain Calinon, 2014 +% http://programming-by-demonstration.org/SylvainCalinon + +nbStates = size(Mu,2); +nbDrawingSeg = 35; +lightcolor = min(color+0.3,1); +t = linspace(-pi, pi, nbDrawingSeg); + +h=[]; +for i=1:nbStates + R = real(sqrtm(1.0.*Sigma(:,:,i))); + X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg); + h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)]; + h = [h plot(Mu(1,:), Mu(2,:), 'x', 'lineWidth', 2, 'markersize', 6, 'color', color)]; +end diff --git a/reproduction_DS.m b/reproduction_DS.m index fc4a85a8ef78a097113b01b83fde7f2c749d2b48..a699846f3a6f337de4b47fb831a7845603e05893 100644 --- a/reproduction_DS.m +++ b/reproduction_DS.m @@ -1,42 +1,42 @@ -function r = reproduction_DS(DataIn, model, r, currPos) -% Reproduction with a virtual spring-damper system with constant impedance parameters -% -% 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" -% } - -nbData = size(DataIn,2); -nbVarOut = model.nbVar - size(DataIn,1); - -%% Reproduction with constant impedance parameters -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -x = currPos; -dx = zeros(nbVarOut,1); -for t=1:nbData - L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; - %Compute acceleration - ddx = -L * [x-r.currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf - %Update velocity and position - dx = dx + ddx * model.dt; - x = x + dx * model.dt; - %Log data - 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 - - - +function r = reproduction_DS(DataIn, model, r, currPos) +% Reproduction with a virtual spring-damper system with constant impedance parameters +% +% 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" +% } + +nbData = size(DataIn,2); +nbVarOut = model.nbVar - size(DataIn,1); + +%% Reproduction with constant impedance parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +x = currPos; +dx = zeros(nbVarOut,1); +for t=1:nbData + L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; + %Compute acceleration + ddx = -L * [x-r.currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf + %Update velocity and position + dx = dx + ddx * model.dt; + x = x + dx * model.dt; + %Log data + 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 + + +