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

Tabs modified.

parent 0f3045f4
......@@ -62,103 +62,17 @@ 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
%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
% 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
......@@ -169,178 +169,3 @@ xlabel('t'); ylabel(['ddx_' num2str(k)]);
%print('-dpng','outTest2.png');
%pause;
%close all;
function demo_TPGMR_LQR01
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of
% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
% relevance of the frame through the task. This information is exploited to generate a new attractor path
% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances
% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
% the spring-damper systems, resulting in a minimal intervention control strategy.
%
% This demo presents the results for a finite horizon LQR.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM
model.nbFrames = 2; %Number of candidate frames of reference
model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
model.dt = 0.01; %Time step
nbRepros = 8; %Number of reproductions with new situations randomly generated
rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...');
% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a
% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
% multiplied by the number of demonstrations (5).
load('data/DataLQR01.mat');
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of tensor GMM with EM:');
model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model);
%% Reproduction with LQR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:s(1).nbData] * model.dt;
for n=1:nbSamples
%Retrieval of attractor path through task-parameterized GMR
a(n) = estimateAttractorPath(DataIn, model, s(n));
r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
end
%% Reproduction with LQR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('New reproductions with LQR...');
for n=1:nbRepros
for m=1:model.nbFrames
%Random generation of new task parameters
id=ceil(rand(2,1)*nbSamples);
w=rand(2); w=w/sum(w);
rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
end
%Retrieval of attractor path through task-parameterized GMR
anew(n) = estimateAttractorPath(DataIn, model, rTmp);
rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
xx = round(linspace(1,64,nbSamples));
clrmap = colormap('jet');
clrmap = min(clrmap(xx,:),.95);
limAxes = [-1.2 0.8 -1.1 0.9];
colPegs = [[.9,.5,.9];[.5,.9,.5]];
%DEMOS
subplot(1,3,1); hold on; box on; title('Demonstrations');
for n=1:nbSamples
%Plot frames
for m=1:model.nbFrames
plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
end
%Plot trajectories
plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%REPROS
subplot(1,3,2); hold on; box on; title('Reproductions with finite horizon LQR');
for n=1:nbSamples
%Plot frames
for m=1:model.nbFrames
plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
end
%Plot Gaussians
plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbSamples
%Plot trajectories
plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%NEW REPROS
subplot(1,3,3); hold on; box on; title('New reproductions with finite horizon LQR');
for n=1:nbRepros
%Plot frames
for m=1:model.nbFrames
plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
end
%Plot Gaussians
plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
end
for n=1:nbRepros
%Plot trajectories
plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
end
axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%print('-dpng','outTest1.png');
%Plot additional information
figure;
%Plot norm of control commands
subplot(1,2,1); hold on;
for n=1:nbRepros
plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('|ddx|');
%Plot strength of the stiffness term
subplot(1,2,2); hold on;
for n=1:nbRepros
plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('|Kp|');
%Plot accelerations due to feedback and feedforward terms
figure; hold on;
n=1; k=1;
plot(r(n).FB(k,:),'r-','linewidth',2);
plot(r(n).FF(k,:),'b-','linewidth',2);
legend('ddx feedback','ddx feedforward');
xlabel('t'); ylabel(['ddx_' num2str(k)]);
%print('-dpng','outTest2.png');
%pause;
%close all;
......@@ -169,178 +169,3 @@ xlabel('t'); ylabel(['ddx_' num2str(k)]);
%print('-dpng','outTest2.png');
%pause;
%close all;
function demo_TPGMR_LQR02
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper
% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of
% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the
% relevance of the frame through the task. This information is exploited to generate a new attractor path
% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances
% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of
% the spring-damper systems, resulting in a minimal intervention control strategy.
%
% This demo presents the results for an infinite horizon LQR.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM
model.nbFrames = 2; %Number of candidate frames of reference
model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
model.dt = 0.01; %Time step
nbRepros = 8; %Number of reproductions with new situations randomly generated
rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...');
% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a
% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
% multiplied by the number of demonstrations (5).
load('data/DataLQR01.mat');
%% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of tensor GMM with EM:');
model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model);
%% Reproduction with LQR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
......@@ -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;
......@@ -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;
......@@ -29,40 +29,3 @@ 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;