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

Tabs modified.

parent 0f3045f4
function model = EM_tensorGMM(Data, model)
% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to
% external parameters represented in the form of candidate coordinate systems.
% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to
% external parameters represented in the form of candidate coordinate systems.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
......@@ -34,131 +34,45 @@ for nbIter=1:nbMaxSteps
[L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below and Eq. (2.0.5) in doc/TechnicalReport.pdf
GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
%M-step
for i=1:model.nbStates
for i=1:model.nbStates
%Update Priors
model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf
for m=1:model.nbFrames
%Matricization/flattening of tensor
DataMat(:,:) = Data(:,m,:);
%Update Mu
%Update Mu
model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf
%Update Sigma (regularization term is optional)
%Update Sigma (regularization term is optional)
DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; %See Eq. (2.0.8) and (2.1.2) in doc/TechnicalReport.pdf
end
end
%Compute average log-likelihood
%Compute average log-likelihood
LL(nbIter) = sum(log(sum(L,1))) / size(L,2); %See Eq. (2.0.4) in doc/TechnicalReport.pdf
%Stop the algorithm if EM converged (small change of LL)
if nbIter>nbMinSteps
if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
disp(['EM converged after ' num2str(nbIter) ' iterations.']);
disp(['EM converged after ' num2str(nbIter) ' iterations.']);
return;
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
%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
%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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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);
%Normalization
GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1);
end
......@@ -12,7 +12,7 @@ for t=1:nbData
%Compute activation weight
%See Eq. (3.0.5) in doc/TechnicalReport.pdf
for i=1:model.nbStates
H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i));
H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i));
end
H(:,t) = H(:,t)/sum(H(:,t));
%Compute expected conditional means
......@@ -25,9 +25,9 @@ for t=1:nbData
%See Eq. (3.0.4) in doc/TechnicalReport.pdf
for i=1:model.nbStates
SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i);
expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)');
expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)');
for j=1:model.nbStates
expSigma(:,:,t) = expSigma(:,:,t) - H(i,t)*H(j,t) * (MuTmp(:,i)*MuTmp(:,j)');
expSigma(:,:,t) = expSigma(:,:,t) - H(i,t)*H(j,t) * (MuTmp(:,i)*MuTmp(:,j)');
end
end
end
......
This diff is collapsed.
This diff is collapsed.
......@@ -4,8 +4,8 @@ function demo_testLQR02
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
......@@ -20,17 +20,17 @@ function demo_testLQR02
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
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
%% 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
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);
......@@ -39,7 +39,7 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
hold on; box on;
hold on; box on;
%Plot target
plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
for n=1:nbRepros
......@@ -48,7 +48,7 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('x_1');
figure;
figure;
%Plot norm of control commands
subplot(1,3,1); hold on;
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
%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;
......@@ -4,8 +4,8 @@ function demo_testLQR03
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
......@@ -20,32 +20,32 @@ function demo_testLQR03
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
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
%% 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.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 = ones(1,1,nbData-100) * 100;
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);
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
%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
%Second call to LQR with finite horizon
r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
end
end
......@@ -68,7 +68,7 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('x_1');
figure;
figure;
%Plot variations
subplot(2,3,[1,4]); hold on;
for n=1:nbRepros
......@@ -96,103 +96,3 @@ 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;
......@@ -4,8 +4,8 @@ function r = estimateAttractorPath(DataIn, model, r)
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
......@@ -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.Priors = model.Priors;
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
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)
% 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));
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.
% o prob: 1 x N vector representing the likelihood of the N datapoints.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
......
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
% http://programming-by-demonstration.org/
......
......@@ -17,11 +17,11 @@ for i=1:model.nbStates
end
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 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);
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 [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
% http://programming-by-demonstration.org/
......@@ -35,12 +35,12 @@ while 1
end
cumdist_old = cumdist;
nbStep = nbStep+1;
% if nbStep>maxIter
% disp(['Maximum number of iterations, ' num2str(maxIter) 'is reached']);
% break;
% end
% if nbStep>maxIter
% disp(['Maximum number of iterations, ' num2str(maxIter) 'is reached']);
% break;
% end
end
%disp(['Kmeans stopped after ' num2str(nbStep) ' steps.']);
%disp(['Kmeans stopped after ' num2str(nbStep) ' steps.']);
......@@ -3,7 +3,7 @@ function h = plotGMM(Mu, Sigma, color)
% 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.
% 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
......
function [Mu, Sigma] = productTPGMM(model, p)
% Sylvain Calinon, Leonel Rozo, 2014
%
%
% Compute the product of Gaussians for a task-parametrized model where the
% set of parameters are stored in the variable 'p'.
% set of parameters are stored in the variable 'p'.
% TP-GMM products
% TP-GMM products
%See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
for i = 1:model.nbStates
% Reallocating
% Reallocating
SigmaTmp = zeros(model.nbVar);
MuTmp = zeros(model.nbVar,1);
% Product of Gaussians
for m = 1 : model.nbFrames
MuP = p(m).A * model.Mu(:,m,i) + p(m).b;
SigmaP = p(m).A * model.Sigma(:,:,m,i) * p(m).A';
for m = 1 : model.nbFrames
MuP = p(m).A * model.Mu(:,m,i) + p(m).b;
SigmaP = p(m).A * model.Sigma(:,:,m,i) * p(m).A';
SigmaTmp = SigmaTmp + inv(SigmaP);
MuTmp = MuTmp + SigmaP\MuP;
MuTmp = MuTmp + SigmaP\MuP;
end
Sigma(:,:,i) = inv(SigmaTmp);
Mu(:,i) = Sigma(:,:,i) * MuTmp;
Mu(:,i) = Sigma(:,:,i) * MuTmp;
end
function r = reproduction_DS(DataIn, model, r, currPos)
% Reproduction with a virtual spring-damper system with constant impedance parameters
% 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:
% 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.",
......@@ -25,14 +25,14 @@ nbVarOut = model.nbVar - size(DataIn,1);
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
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.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));
......
......@@ -4,8 +4,8 @@ function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor,
% Authors: Sylvain Calinon and Danilo Bruno, 2014
% http://programming-by-demonstration.org
%
% This source code is given for free! In exchange, we would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
% This source code is given for free! In exchange, we 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.",
......@@ -28,7 +28,7 @@ B = kron([0; 1], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf
%Initialize Q and R weighting matrices
Q = zeros(nbVarOut*2,nbVarOut*2,nbData);
for t=1:nbData
Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
end
R = eye(nbVarOut) * rFactor;
......@@ -43,8 +43,8 @@ d = zeros(nbVarOut*2, nbData); %For optional feedforward term computation
%Feedback term
L = zeros(nbVarOut, nbVarOut*2, nbData);
%Compute P_T from the desired final feedback gains L_T,
P(:,:,nbData) = Pfinal;
%Compute P_T from the desired final feedback gains L_T,
P(:,:,nbData) = Pfinal;
%Compute derivative of target path
%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation
......@@ -77,17 +77,17 @@ dx = zeros(nbVarOut,1);
for t=1:nbData
%Compute acceleration (with only feedback term)
%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];