Commit 0232c4c7 authored by Sylvain CALINON's avatar Sylvain CALINON

New demo_batchLQR_viapoints* examples

parent dbd1195f
function demo_batchLQR_viapoints01
% Keypoint-based motion through MPC, with a GMM encoding of position and velocity
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @inproceedings{Berio17GI,
% author="Berio, D. and Calinon, S. and Fol Leymarie, F.",
% title="Generating Calligraphic Trajectories with Model Predictive Control",
% booktitle="Proc. 43rd Conf. on Graphics Interface",
% year="2017",
% month="May",
% address="Edmonton, AL, Canada",
% pages="132--139",
% doi="10.20380/GI2017.17"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbSamples = 5; %Number of demonstrations
nbRepros = 1; %Number of reproductions in new situations
nbData = 200; %Number of datapoints
model.nbStates = 4; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
model.rfactor = 1E-8; %Control cost in LQR
model.dt = 0.01; %Time step duration
model.params_diagRegFact = 1E-8; %Control cost in LQR
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
R = kron(eye(nbData-1),R);
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Integration with higher order Taylor series expansion
A1d = zeros(model.nbDeriv);
for i=0:model.nbDeriv-1
A1d = A1d + diag(ones(model.nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
end
B1d = zeros(model.nbDeriv,1);
for i=1:model.nbDeriv
B1d(model.nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
end
A = kron(A1d, eye(model.nbVarPos)); %Discrete nD
B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
%Build Sx and Su matrices for batch LQR, see Eq. (35)
Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
Sx = kron(ones(nbData,1),eye(model.nbVar));
M = B;
for n=2:nbData
id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
Sx(id1,:) = Sx(id1,:) * A;
id1 = (n-1)*model.nbVar+1:n*model.nbVar;
id2 = 1:(n-1)*model.nbVarPos;
Su(id1,id2) = M;
M = [A*M(:,1:model.nbVarPos), M]; %Also M = [A^(n-1)*B, M] or M = [Sx(id1,:)*B, M]
end
%% Generate keypoints from handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/2Dletters/M.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
for m=1:model.nbDeriv
if m==1
dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
else
dTmp = gradient(dTmp) / model.dt; %Compute derivatives
end
s(n).Data = [s(n).Data; dTmp];
end
Data = [Data s(n).Data];
end
%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation...');
%model = init_GMM_kmeans(Data, model);
model = init_GMM_kbins(Data, model, nbSamples);
%Refinement of parameters
[model, H] = EM_GMM(Data, model);
%Precomputation of inverses
for i=1:model.nbStates
model.invSigma(:,:,i) = inv(model.Sigma(:,:,i));
end
%Set list of states according to first demonstration (alternatively, an HSMM can be used)
[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1
%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q
MuQ = zeros(model.nbVar*nbData, 1);
Q = zeros(model.nbVar*nbData);
qCurr = qList(1);
qT = [];
for t=1:nbData
if qCurr~=qList(t) || t==nbData
id = (t-1)*model.nbVar+1:t*model.nbVar;
MuQ(id) = model.Mu(:,qCurr);
Q(id,id) = model.invSigma(:,:,qCurr);
qCurr = qList(t);
qT = [qT, t];
end
end
%% Batch LQR reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Set matrices to compute the damped weighted least squares estimate, see Eq. (37)
SuQ = Su' * Q;
Rq = SuQ * Su + R;
%Reproductions
for n=1:nbRepros
X = Data(:,1) + [randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
rq = SuQ * (MuQ-Sx*X);
u = Rq \ rq; %Can also be computed with u = lscov(Rq, rq);
r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [.5 .5 .5], .5);
for n=1:nbRepros
plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
end
axis equal;
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$'};
figure('position',[710 10 600 700],'color',[1 1 1]);
for j=1:4
subplot(4,1,j); hold on;
for n=1:nbRepros
plot(r(n).Data(j,:), '-','linewidth',1,'color',[.8 0 0]);
end
for t=1:length(qT)
plot([qT(t),qT(t)], [min(Data(j,:)),max(Data(j,:))], '-','color',[.5 .5 .5]);
end
ylabel(labList{j},'fontsize',14,'interpreter','latex');
end
xlabel('$t$','fontsize',14,'interpreter','latex');
%print('-dpng','graphs/demo_batchLQR_viapoints01.png');
%pause;
%close all;
function demo_batchLQR_viapoints02
% Keypoint-based motion through MPC, with a GMM encoding of only position information
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @inproceedings{Berio17GI,
% author="Berio, D. and Calinon, S. and Fol Leymarie, F.",
% title="Generating Calligraphic Trajectories with Model Predictive Control",
% booktitle="Proc. 43rd Conf. on Graphics Interface",
% year="2017",
% month="May",
% address="Edmonton, AL, Canada",
% pages="132--139",
% doi="10.20380/GI2017.17"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbSamples = 5; %Number of demonstrations
nbRepros = 1; %Number of reproductions in new situations
nbData = 200; %Number of datapoints
model.nbStates = 4; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 1; %Number of static & dynamic features (D=1 for just x)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
model.dt = 0.01; %Time step duration
model.rfactor = 1E-8; %Control cost in LQR
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
R = kron(eye(nbData-1),R);
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbDeriv = model.nbDeriv + 1; %For definition of dynamical system
%Integration with higher order Taylor series expansion
A1d = zeros(nbDeriv);
for i=0:nbDeriv-1
A1d = A1d + diag(ones(nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
end
B1d = zeros(nbDeriv,1);
for i=1:nbDeriv
B1d(nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
end
A = kron(A1d, eye(model.nbVarPos)); %Discrete nD
B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
C = kron([1, 0], eye(model.nbVarPos));
%Build CSx and CSu matrices for batch LQR, see Eq. (35)
CSu = zeros(model.nbVarPos*nbData, model.nbVarPos*(nbData-1));
CSx = kron(ones(nbData,1), [eye(model.nbVarPos) zeros(model.nbVarPos)]);
M = B;
for n=2:nbData
id1 = (n-1)*model.nbVarPos+1:n*model.nbVarPos;
CSx(id1,:) = CSx(id1,:) * A;
id1 = (n-1)*model.nbVarPos+1:n*model.nbVarPos;
id2 = 1:(n-1)*model.nbVarPos;
CSu(id1,id2) = C * M;
M = [A*M(:,1:model.nbVarPos), M]; %Also M = [A^(n-1)*B, M] or M = [Sx(id1,:)*B, M]
end
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/2Dletters/M.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
for m=1:model.nbDeriv
if m==1
dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
else
dTmp = gradient(dTmp) / model.dt; %Compute derivatives
end
s(n).Data = [s(n).Data; dTmp];
end
Data = [Data s(n).Data];
end
%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation...');
%model = init_GMM_kmeans(Data,model);
model = init_GMM_kbins(Data,model,nbSamples);
%Refinement of parameters
[model, H] = EM_GMM(Data, model);
%Precomputation of inverses
for i=1:model.nbStates
model.invSigma(:,:,i) = inv(model.Sigma(:,:,i));
end
%Set list of states according to first demonstration (alternatively, an HSMM can be used)
[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1
%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q
MuQ = zeros(model.nbVar*nbData, 1);
Q = zeros(model.nbVar*nbData);
qCurr = qList(1);
qT = [];
for t=1:nbData
if qCurr~=qList(t) || t==nbData
id = (t-1)*model.nbVar+1:t*model.nbVar;
MuQ(id) = model.Mu(:,qCurr);
Q(id,id) = model.invSigma(:,:,qCurr);
qCurr = qList(t);
qT = [qT, t];
end
end
%% Batch LQR reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Set matrices to compute the damped weighted least squares estimate
SuQ = CSu' * Q;
Rq = SuQ * CSu + R;
%Reproductions
for n=1:nbRepros
X = [Data(:,1)+randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
rq = SuQ * (MuQ-CSx*X);
u = Rq \ rq; %Can also be computed with u = lscov(Rq, rq);
r(n).Data = reshape(CSx*X+CSu*u, model.nbVarPos, nbData);
end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [.5 .5 .5], .5);
for n=1:nbRepros
plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
end
axis equal;
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$'};
figure('position',[710 10 600 700],'color',[1 1 1]);
for j=1:2
subplot(2,1,j); hold on;
for n=1:nbRepros
plot(r(n).Data(j,:), '-','linewidth',1,'color',[.8 0 0]);
end
for t=1:length(qT)
plot([qT(t),qT(t)], [min(Data(j,:)),max(Data(j,:))], '-','color',[.5 .5 .5]);
end
ylabel(labList{j},'fontsize',14,'interpreter','latex');
end
xlabel('$t$','fontsize',14,'interpreter','latex');
%print('-dpng','graphs/demo_batchLQR_viapoints02.png');
%pause;
%close all;
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment