Commit 789def57 authored by Milad Malekzadeh's avatar Milad Malekzadeh

Tabs modified.

parent 0f3045f4
function model = EM_tensorGMM(Data, model) function model = EM_tensorGMM(Data, model)
% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. % 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 % 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. % external parameters represented in the form of candidate coordinate systems.
% %
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -34,131 +34,45 @@ for nbIter=1:nbMaxSteps ...@@ -34,131 +34,45 @@ for nbIter=1:nbMaxSteps
[L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below and Eq. (2.0.5) in doc/TechnicalReport.pdf [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); GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
%M-step %M-step
for i=1:model.nbStates for i=1:model.nbStates
%Update Priors %Update Priors
model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf
for m=1:model.nbFrames for m=1:model.nbFrames
%Matricization/flattening of tensor %Matricization/flattening of tensor
DataMat(:,:) = Data(:,m,:); DataMat(:,:) = Data(:,m,:);
%Update Mu %Update Mu
model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf
%Update Sigma (regularization term is optional) %Update Sigma (regularization term is optional)
DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData); 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 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
end end
%Compute average log-likelihood %Compute average log-likelihood
LL(nbIter) = sum(log(sum(L,1))) / size(L,2); %See Eq. (2.0.4) in doc/TechnicalReport.pdf 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) %Stop the algorithm if EM converged (small change of LL)
if nbIter>nbMinSteps if nbIter>nbMinSteps
if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1 if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
disp(['EM converged after ' num2str(nbIter) ' iterations.']); disp(['EM converged after ' num2str(nbIter) ' iterations.']);
return; return;
end end
end end
end end
disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']); disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
end end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [L, GAMMA, GAMMA0] = computeGamma(Data, model) function [L, GAMMA, GAMMA0] = computeGamma(Data, model)
%See Eq. (2.0.5) in doc/TechnicalReport.pdf %See Eq. (2.0.5) in doc/TechnicalReport.pdf
nbData = size(Data, 3); nbData = size(Data, 3);
L = ones(model.nbStates, nbData); L = ones(model.nbStates, nbData);
GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData); GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
for m=1:model.nbFrames for m=1:model.nbFrames
DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
for i=1:model.nbStates for i=1:model.nbStates
GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i)); GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))'; L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))';
end
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
% 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 end
%Normalization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1);
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 end
...@@ -12,7 +12,7 @@ for t=1:nbData ...@@ -12,7 +12,7 @@ for t=1:nbData
%Compute activation weight %Compute activation weight
%See Eq. (3.0.5) in doc/TechnicalReport.pdf %See Eq. (3.0.5) in doc/TechnicalReport.pdf
for i=1:model.nbStates for i=1:model.nbStates
H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i)); H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i));
end end
H(:,t) = H(:,t)/sum(H(:,t)); H(:,t) = H(:,t)/sum(H(:,t));
%Compute expected conditional means %Compute expected conditional means
...@@ -25,9 +25,9 @@ for t=1:nbData ...@@ -25,9 +25,9 @@ for t=1:nbData
%See Eq. (3.0.4) in doc/TechnicalReport.pdf %See Eq. (3.0.4) in doc/TechnicalReport.pdf
for i=1:model.nbStates for i=1:model.nbStates
SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i); SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i);
expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)'); expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)');
for j=1:model.nbStates for j=1:model.nbStates
expSigma(:,:,t) = expSigma(:,:,t) - H(i,t)*H(j,t) * (MuTmp(:,i)*MuTmp(:,j)'); expSigma(:,:,t) = expSigma(:,:,t) - H(i,t)*H(j,t) * (MuTmp(:,i)*MuTmp(:,j)');
end end
end end
end end
......
This diff is collapsed.
This diff is collapsed.
...@@ -4,8 +4,8 @@ function demo_testLQR02 ...@@ -4,8 +4,8 @@ function demo_testLQR02
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -20,17 +20,17 @@ function demo_testLQR02 ...@@ -20,17 +20,17 @@ function demo_testLQR02
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
model.dt = 0.01; %Time step model.dt = 0.01; %Time step
nbData = 1000; %Number of datapoints nbData = 1000; %Number of datapoints
nbRepros = 1; %Number of reproductions with new situations randomly generated nbRepros = 1; %Number of reproductions with new situations randomly generated
rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
%% Reproduction with LQR %% Reproduction with LQR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...'); disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt; DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData); a.currTar = ones(1,nbData);
a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u
for n=1:nbRepros for n=1:nbRepros
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
...@@ -39,7 +39,7 @@ end ...@@ -39,7 +39,7 @@ end
%% Plots %% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]); figure('position',[20,50,1300,500]);
hold on; box on; hold on; box on;
%Plot target %Plot target
plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
for n=1:nbRepros for n=1:nbRepros
...@@ -48,7 +48,7 @@ for n=1:nbRepros ...@@ -48,7 +48,7 @@ for n=1:nbRepros
end end
xlabel('t'); ylabel('x_1'); xlabel('t'); ylabel('x_1');
figure; figure;
%Plot norm of control commands %Plot norm of control commands
subplot(1,3,1); hold on; subplot(1,3,1); hold on;
for n=1:nbRepros for n=1:nbRepros
...@@ -72,77 +72,3 @@ r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071 ...@@ -72,77 +72,3 @@ r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071
%pause; %pause;
%close all; %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;
...@@ -4,8 +4,8 @@ function demo_testLQR03 ...@@ -4,8 +4,8 @@ function demo_testLQR03
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -20,32 +20,32 @@ function demo_testLQR03 ...@@ -20,32 +20,32 @@ function demo_testLQR03
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
model.dt = 0.01; %Time step model.dt = 0.01; %Time step
nbData = 400; %Number of datapoints nbData = 400; %Number of datapoints
nbRepros = 2; %Number of reproductions with new situations randomly generated nbRepros = 2; %Number of reproductions with new situations randomly generated
rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
%% Reproduction with LQR %% Reproduction with LQR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...'); disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt; DataIn = [1:nbData] * model.dt;
%a.currTar = ones(1,nbData); %a.currTar = ones(1,nbData);
a.currTar = linspace(100,100,nbData); a.currTar = linspace(100,100,nbData);
%a.currTar = sin(linspace(0,8*pi,nbData)); %a.currTar = sin(linspace(0,8*pi,nbData));
%a.currSigma = ones(1,1,nbData); %a.currSigma = ones(1,1,nbData);
a.currSigma = ones(1,1,nbData-100) * 100; a.currSigma = ones(1,1,nbData-100) * 100;
a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1; a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1;
aFinal.currTar = a.currTar(:,end); aFinal.currTar = a.currTar(:,end);
aFinal.currSigma = a.currSigma(:,:,end); aFinal.currSigma = a.currSigma(:,:,end);
for n=1:nbRepros for n=1:nbRepros
if n==1 if n==1
r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
else else
%First call to LQR to get an estimate of the final feedback terms %First call to LQR to get an estimate of the final feedback terms
[~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor); [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
%Second call to LQR with finite horizon %Second call to LQR with finite horizon
r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal); r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
end end
end end
...@@ -68,7 +68,7 @@ for n=1:nbRepros ...@@ -68,7 +68,7 @@ for n=1:nbRepros
end end
xlabel('t'); ylabel('x_1'); xlabel('t'); ylabel('x_1');
figure; figure;
%Plot variations %Plot variations
subplot(2,3,[1,4]); hold on; subplot(2,3,[1,4]); hold on;
for n=1:nbRepros for n=1:nbRepros
...@@ -96,103 +96,3 @@ xlabel('t'); ylabel('kv'); ...@@ -96,103 +96,3 @@ xlabel('t'); ylabel('kv');
%pause; %pause;
%close all; %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;
...@@ -4,8 +4,8 @@ function r = estimateAttractorPath(DataIn, model, r) ...@@ -4,8 +4,8 @@ function r = estimateAttractorPath(DataIn, model, r)
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -25,44 +25,7 @@ out = in(end)+1:model.nbVar; ...@@ -25,44 +25,7 @@ out = in(end)+1:model.nbVar;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[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.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.Priors = model.Priors;
r.nbStates = model.nbStates; 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 [r.currTar, r.currSigma] = GMR(r, DataIn, in, out); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
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
...@@ -6,25 +6,7 @@ function prob = gaussPDF(Data, Mu, Sigma) ...@@ -6,25 +6,7 @@ function prob = gaussPDF(Data, Mu, Sigma)
% o Mu: D x 1 vector representing the center of the Gaussian. % 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. % o Sigma: D x D array representing the covariance matrix of the Gaussian.
% Output ----------------------------------------------------------------- % Output -----------------------------------------------------------------
% o prob: 1 x N vector representing the likelihood of the N datapoints. % 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));
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 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
......
function model = init_tensorGMM_kmeans(Data, model) function model = init_tensorGMM_kmeans(Data, model)
% Initialization of the model with k-means. % Initialization of the model with k-means.
% Authors: Sylvain Calinon, Tohid Alizadeh, 2013 % Authors: Sylvain Calinon, Tohid Alizadeh, 2013
% http://programming-by-demonstration.org/ % http://programming-by-demonstration.org/
......
...@@ -17,11 +17,11 @@ for i=1:model.nbStates ...@@ -17,11 +17,11 @@ for i=1:model.nbStates
end end
model.Priors = model.Priors / sum(model.Priors); model.Priors = model.Priors / sum(model.Priors);
%Reshape GMM parameters into a tensor %Reshape GMM parameters into a tensor
for m=1:model.nbFrames for m=1:model.nbFrames
for i=1:model.nbStates for i=1:model.nbStates
model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); 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); model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
end end
end end
function [idList, Mu] = kmeansClustering(Data, nbStates) function [idList, Mu] = kmeansClustering(Data, nbStates)
% Initialization of the model with k-means. % Initialization of the model with k-means.
% Authors: Sylvain Calinon, Tohid Alizadeh, 2013 % Authors: Sylvain Calinon, Tohid Alizadeh, 2013
% http://programming-by-demonstration.org/ % http://programming-by-demonstration.org/