Commit adbda65c authored by Sylvain CALINON's avatar Sylvain CALINON

Publication reference updated

parent 65b56cf3
......@@ -277,7 +277,7 @@ If you find PbDlib useful for your research, please cite the related publication
</p>
<p><a name="ref-3">
[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds: Applications for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
</a><br>
[[pdf]](http://calinon.ch/papers/Calinon-RAM2020.pdf)
[[bib]](http://calinon.ch/papers/Calinon-RAM2020.bib)
......
function demo_MPC03
% Batch computation of linear quadratic tracking problem, by tracking position and velocity references.
% Batch computation of linear quadratic tracking problem, by tracking only position references.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
......@@ -42,10 +42,10 @@ nbData = 200; %Number of datapoints
model.nbStates = 8; %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.nbDeriv = 1; %Number of static & dynamic features (nbDeriv=1 for just x)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
model.rfactor = 1E-6; %Control cost in LQR
model.dt = 0.01; %Time step duration
model.rfactor = 1E-3; %Control cost in LQR
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
......@@ -54,27 +54,25 @@ R = kron(eye(nbData-1),R);
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbDeriv = model.nbDeriv + 1; %For definition of dynamical system
% %Integration with Euler method
% Ac1d = diag(ones(nbDeriv-1,1),1);
% Bc1d = [zeros(nbDeriv-1,1); 1];
% A = kron(eye(nbDeriv)+Ac1d*model.dt, eye(model.nbVarPos));
% Ac1d = diag(ones(model.nbDeriv-1,1),1);
% Bc1d = [zeros(model.nbDeriv-1,1); 1];
% A = kron(eye(model.nbDeriv)+Ac1d*model.dt, eye(model.nbVarPos));
% B = kron(Bc1d*model.dt, eye(model.nbVarPos));
% C = kron([1, 0], eye(model.nbVarPos));
%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
A1d = zeros(model.nbDeriv);
for i=0:model.nbDeriv-1
A1d = A1d + diag(ones(model.nbDeriv-i,1),i) .* model.dt^i ./ 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
B1d = zeros(model.nbDeriv,1);
for i=1:model.nbDeriv
B1d(model.nbDeriv-i+1) = model.dt^i ./ 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));
C = kron([1, zeros(1,model.nbDeriv-1)], eye(model.nbVarPos));
% %Conversion with control toolbox
% Ac1d = diag(ones(nbDeriv-1,1),1); %Continuous 1D
......@@ -86,9 +84,9 @@ C = kron([1, 0], eye(model.nbVarPos));
% C = kron([1, 0], eye(model.nbVarPos));
%Build CSx and CSu matrices for batch LQR, see Eq. (35)
%Build CSx and CSu matrices for batch LQR
CSu = zeros(model.nbVarPos*nbData, model.nbVarPos*(nbData-1));
CSx = kron(ones(nbData,1), [eye(model.nbVarPos) zeros(model.nbVarPos)]);
CSx = kron(ones(nbData,1), [eye(model.nbVarPos) zeros(model.nbVarPos, model.nbVarPos*(model.nbDeriv-1))]);
M = B;
for n=2:nbData
id1 = (n-1)*model.nbVarPos+1:n*model.nbVarPos;
......@@ -105,15 +103,7 @@ end
load('data/2Dletters/E.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
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
Data = [Data s(n).Data];
end
......@@ -121,8 +111,8 @@ end
%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation...');
%model = init_GMM_kmeans(Data,model);
model = init_GMM_kbins(Data,model,nbSamples);
%model = init_GMM_kmeans(Data, model);
model = init_GMM_kbins(Data, model, nbSamples);
%Refinement of parameters
[model, H] = EM_GMM(Data, model);
......@@ -148,11 +138,11 @@ CSuInvSigmaQ = CSu' / SigmaQ;
Rq = CSuInvSigmaQ * CSu + R;
%Reproductions
for n=1:nbRepros
%X = [Data(:,1)+randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)];
X = [Data(:,1); zeros(model.nbVarPos,1)];
rq = CSuInvSigmaQ * (MuQ-CSx*X);
%x = [Data(:,1)+randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)];
x = [Data(:,1); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
rq = CSuInvSigmaQ * (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);
r(n).Data = reshape(CSx*x+CSu*u, model.nbVarPos, nbData);
end
......@@ -200,20 +190,20 @@ end
%% Stochastic sampling by exploiting distribution on x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbEigs = 2; %Number of principal eigencomponents to keep
X = [Data(:,1); zeros(model.nbVarPos,1)];
x = [Data(:,1); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
CSuInvSigmaQ = CSu' / SigmaQ;
Rq = CSuInvSigmaQ * CSu + R;
rq = CSuInvSigmaQ * (MuQ-CSx*X);
rq = CSuInvSigmaQ * (MuQ-CSx*x);
u = Rq \ rq;
Mu = CSx * X + CSu * u;
Mu = CSx * x + CSu * u;
mse = 1; %abs(MuQ'/SigmaQ*MuQ - rq'/Rq*rq) ./ (model.nbVarPos*nbData);
Sigma = CSu/Rq*CSu' * mse + eye(nbData*model.nbVar) * 0E-10;
Sigma = CSu/Rq*CSu' * mse + eye(nbData*model.nbVarPos) * 0E-10;
%[V,D] = eigs(Sigma);
[V,D] = eig(Sigma);
for n=1:nbNewRepros
%xtmp = Mu + V(:,1:nbEigs) * D(1:nbEigs,1:nbEigs).^.5 * randn(nbEigs,1) * 0.1;
xtmp = Mu + V * D.^.5 * randn(size(D,1),1) * 0.9;
rnew(n).Data = reshape(xtmp, model.nbVar, nbData); %Reshape data for plotting
rnew(n).Data = reshape(xtmp, model.nbVarPos, nbData); %Reshape data for plotting
end
......
......@@ -35,7 +35,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 4; %Number of Gaussians in the GMM
model.nbStates = 6; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static & dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
model.nbVar = model.nbVarPos * (model.nbDeriv+1); %Dimension of augmented state vector
......@@ -135,11 +135,11 @@ Q(1:12,1:12)
SuInvSigmaQ = Su' * Q;
Rq = SuInvSigmaQ * Su + R;
for n=1:nbRepros
X = Data(:,1);
%X = [Data(1:4,1); model.Mu(5:6,end)];
rq = SuInvSigmaQ * (MuQ-Sx*X);
x = Data(:,1);
%x = [Data(1:4,1); model.Mu(5:6,end)];
rq = SuInvSigmaQ * (MuQ-Sx*x);
u = Rq \ rq;
r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
r(n).Data = reshape(Sx*x+Su*u, model.nbVar, nbData);
r(n).u = reshape(u, model.nbVarPos, nbData-1);
end
......
......@@ -36,48 +36,31 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbVarPos = 2; %Dimension of position data (here: x1,x2)
nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
nbDeriv = 1; %Number of static & dynamic features (nbDeriv=2 for [x,dx])
nbData = 200; %Number of datapoints in a trajectory
dt = 0.01; %Time step duration
rfactor = 1E2; %Control cost in LQR
rfactor = 1E2; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor;
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Integration with higher order Taylor series expansion
A1d = zeros(nbDeriv);
for i=0:nbDeriv-1
A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
A1d = A1d + diag(ones(nbDeriv-i,1),i) .* dt^i ./ factorial(i); %Discrete 1D
end
B1d = zeros(nbDeriv,1);
for i=1:nbDeriv
B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
B1d(nbDeriv-i+1) = dt^i ./ factorial(i); %Discrete 1D
end
A = kron(A1d, speye(nbVarPos)); %Discrete nD
B = kron(B1d, speye(nbVarPos)); %Discrete nD
C = kron([1, 0], eye(nbVarPos));
% %Construct Su and Sx matrices (transfer matrices in batch LQR)
% Su = zeros(nbVar*nbData, nbVarPos*(nbData-1));
% Sx = kron(ones(nbData,1),eye(nbVar));
% M = B;
% for n=2:nbData
% %Build Sx matrix
% id1 = (n-1)*nbVar+1:nbData*nbVar;
% Sx(id1,:) = Sx(id1,:) * A;
% %Build Su matrix
% id1 = (n-1)*nbVar+1:n*nbVar;
% id2 = 1:(n-1)*nbVarPos;
% Su(id1,id2) = M;
% M = [A*M(:,1:nbVarPos), M];
% end
A = kron(A1d, eye(nbVarPos)); %Discrete nD
B = kron(B1d, eye(nbVarPos)); %Discrete nD
C = kron([1, zeros(1,nbDeriv-1)], eye(nbVarPos));
%Construct CSu and CSx matrices (transfer matrices in batch LQR)
%Build CSx and CSu matrices for batch LQR
CSu = zeros(nbVarPos*nbData, nbVarPos*(nbData-1));
CSx = kron(ones(nbData,1), [eye(nbVarPos) zeros(nbVarPos)]);
CSx = kron(ones(nbData,1), [eye(nbVarPos) zeros(nbVarPos, nbVarPos*(nbDeriv-1))]);
M = B;
for n=2:nbData
id1 = (n-1)*nbVarPos+1:n*nbVarPos;
......@@ -117,7 +100,7 @@ end
Psi = kron(phi, eye(nbVarPos));
% w = (Psi' * Psi + eye(nbFct).*1E-18) \ Psi' * x;
% w = (Psi' * Psi + eye(nbFct*nbVarPos).*1E-18) \ Psi' * x;
w = pinv(Psi) * x;
%Distribution in parameter space
......@@ -132,12 +115,28 @@ Q = Psi / Sigma_w * Psi'; %Precision matrix
%% Reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0 = [x(1:2,1)+.2; zeros(nbVarPos,1)];
% u = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx*x0));
% rx = reshape(Sx*x0+Su*u, nbVarPos, nbData);
u = (CSu' * Q * CSu + R) \ (CSu' * Q * (MuQ - CSx*x0));
rx = reshape(CSx*x0+CSu*u, nbVarPos, nbData);
x0 = [x(1:2,1)+.2; zeros(nbVarPos*(nbDeriv-1),1)];
% % u = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx*x0));
% % rx = reshape(Sx*x0+Su*u, nbVarPos, nbData);
% u = (CSu' * Q * CSu + R) \ (CSu' * Q * (MuQ - CSx*x0));
% rx = reshape(CSx*x0+CSu*u, nbVarPos, nbData);
%Alternative computation highlighting the linear relation to Mu_w and x0 in the solution
A0 = (CSu' * Q * CSu + R) \ CSu' * Q;
A1 = CSu * A0 * Psi;
A2 = CSx - CSu * A0 * CSx;
rx = reshape(A1 * Mu_w + A2 * x0, nbVarPos, nbData);
%Comparison with Gaussian conditioning
in = 1:nbVarPos;
out = nbVarPos+1:nbVarPos*nbData;
Sigma = Psi * Sigma_w * Psi';
rx_out = MuQ(out) + Sigma(out,in) / Sigma(in,in) * (x0(1:nbVarPos) - MuQ(in));
% rx_out = MuQ(out) - Q(out,out) \ Q(out,in) * (x0(1:nbVarPos) - MuQ(in));
rx2 = reshape([x0(1:nbVarPos); rx_out], nbVarPos, nbData);
% %Computation of covariances
% uSigma = inv(Rq) .* 1E-1;
% xSigma = Su * uSigma * Su';
......@@ -150,8 +149,9 @@ for n=1:nbSamples
plot(x(1:2:end,n), x(2:2:end,n), '-','linewidth',4,'color',[.7,.7,.7]);
end
plot(MuQ(1:2:end), MuQ(2:2:end), '-','linewidth',4,'color',[.8 0 0]);
plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0,0,0]);
plot(rx(1,1), rx(2,1), '.','markersize',28,'color',[0,0,0]);
plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0,0,0]);
plot(rx2(1,:), rx2(2,:), '-','linewidth',4,'color',[0,0,.8]);
axis equal;
%Timeline Plots
......@@ -180,6 +180,12 @@ end
set(gca,'xtick',[],'ytick',[]);
xlabel('$t$','interpreter','latex','fontsize',34); ylabel('$\phi_k$','interpreter','latex','fontsize',34);
% %Psi*Psi' plot
% figure; hold on; axis off; title('\Psi\Psi^T','fontsize',16);
% colormap(flipud(gray));
% imagesc(abs(Psi * Psi'));
% axis tight; axis square; axis ij;
% print('-dpng','graphs/MPC_MP01.png');
pause;
close all;
\ No newline at end of file
function demo_MPC_fullQ03
% Batch LQT exploiting full Q matrix to constrain the motion of two agents in a ballistic task.
% Batch LQT problem exploiting full Q matrix to constrain the motion of two agents in a simple ballistic task.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
......@@ -37,7 +37,7 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 200; %Number of datapoints
nbAgents = 2; %Number of agents
nbVarPos = 2 * nbAgents; %Dimension of position data (here: x1,x2)
nbVarPos = 2 * nbAgents; %Dimension of position data (here: x1,x2 for the two agents)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * (nbDeriv+1); %Dimension of state vector
dt = 1E-2; %Time step duration
......@@ -51,7 +51,7 @@ R = speye((nbData-1)*nbVarPos) * rfactor; %Control cost matrix
% Ad0 = speye(6) + Ac0 * dt;
% Ad0 = kron(Ad0, speye(nbAgents)); %Discrete nD for several agents
Ac1 = kron([0, 1, 0; 0, 0, 5; 0, 0, 0], speye(2));
Ac1 = kron([0, 1, 0; 0, 0, 1; 0, 0, 0], speye(2));
Ad1 = speye(6) + Ac1 * dt;
Bc1 = kron([0; 1; 0], speye(2));
......@@ -70,8 +70,8 @@ Bd = kron(Bd1, speye(nbAgents)); %Discrete nD for several agents
% return
%Build Sx and Su transfer matrices(for heterogeneous A and B)
A = zeros(nbVar,nbVar,nbData);
B = zeros(nbVar,nbVarPos,nbData);
A = zeros(nbVar,nbVar,nbData-1);
B = zeros(nbVar,nbVarPos,nbData-1);
for t=1:nbData
A(:,:,t) = Ad;
% A(5:8,9:12,t) = 0; %no gravity on agents
......@@ -80,10 +80,10 @@ for t=1:nbData
if t==1
% A(:,:,t) = Ad0;
% B(:,:,t) = Bd;
B(:,1:2,t) = Bd(:,1:2);
B(:,1:2,t) = Bd(:,1:2); %Agent 1 is only allowed to apply control commands at first time step
end
if t==1
B(:,3:4,t) = Bd(:,3:4);
B(:,3:4,t) = Bd(:,3:4); %Agent 2 is only allowed to apply control commands at first time step
end
end
[Su, Sx] = transferMatrices(A, B);
......@@ -93,19 +93,21 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MuQ = zeros(nbVar*nbData,1);
Q = zeros(nbVar*nbData);
id = [1:nbVarPos] + (nbData-1) * nbVar;
%Constraining the two agents to meet at tEvent
tEvent = (nbData-1);
id = [1:nbVarPos] + tEvent * nbVar;
Q(id,id) = eye(nbVarPos);
MuQ(id) = rand(nbVarPos,1);
%Constraining the two agents to meet at the end of the motion
MuQ(id(3:4)) = MuQ(id(1:2)); %Proposed meeting point for the two agents
MuQ(id(3:4)) = MuQ(id(1:2)); %Proposed meeting point for the two agents (does not need to be this point)
Q(id(1:2), id(3:4)) = -eye(2)*1E0;
Q(id(3:4), id(1:2)) = -eye(2)*1E0;
%% Batch LQR reproduction
%% Batch LQT reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0 = [rand(4,1); zeros(4,1); 0; -9.81*1E-2; 0; -9.81*0E-2];
x0 = [rand(4,1); zeros(4,1); 0; -9.81*1E-2; 0; -9.81*1E-2]; %Both agents have random initial positions and are affected by gravity
u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
......@@ -116,21 +118,27 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
%Agent 1
% for t=1:nbData
% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-8, [.2 .2 .2], .03); %Plot uncertainty
% end
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',35,'color',[0 0 0]);
h(1) = plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',25,'color',[0 0 0]);
plot2DArrow(rx(1:2,1), u(1:2).*1E-2, [.8 0 0]);
% plot(rx(1,end-3), rx(2,end-2), '.','markersize',25,'color',[0 .6 0]); %meeting point
% plot(MuQ(id(1)), MuQ(id(2)), '.','markersize',35,'color',[.8 0 0]);
%Agent 2
% for t=1:nbData
% plotGMM(rx(3:4,t), xSigma(nbVar*(t-1)+[3,4],nbVar*(t-1)+[3,4]).*1E-8, [.2 .2 .2], .03); %Plot uncertainty
% end
plot(rx(3,:), rx(4,:), '-','linewidth',2,'color',[.6 .6 .6]);
plot(rx(3,1), rx(4,1), '.','markersize',35,'color',[.6 .6 .6]);
plot(rx(3,end-1), rx(4,end), '.','markersize',35,'color',[.4 .8 .4]); %meeting point
h(2) = plot(rx(3,:), rx(4,:), '-','linewidth',2,'color',[.6 .6 .6]);
plot(rx(3,1), rx(4,1), '.','markersize',25,'color',[.6 .6 .6]);
plot2DArrow(rx(3:4,1), u(3:4).*1E-2, [.8 0 0]);
h(3) = plot(rx(3,tEvent+1), rx(4,tEvent+1), '.','markersize',25,'color',[.4 .8 .4]); %meeting point
legend(h,{'Agent 1','Agent 2','End point'});
% plot(MuQ(id(3)), MuQ(id(4)), '.','markersize',35,'color',[1 .6 .6]);
axis equal;
% print('-dpng','graphs/MPC_fullQ03a.png');
......@@ -150,18 +158,15 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%
function [Su, Sx] = transferMatrices(A, B)
[nbVarX, nbVarU, nbData] = size(B);
Su = zeros(nbVarX*nbData, nbVarU*(nbData-1));
nbData = nbData+1;
Sx = kron(ones(nbData,1), speye(nbVarX));
M = speye(nbVarX);
N = [];
for t=2:nbData
id1 = (t-1)*nbVarX+1:nbData*nbVarX;
Sx(id1,:) = Sx(id1,:) * A(:,:,t-1);
id1 = (t-1)*nbVarX+1:t*nbVarX;
id2 = 1:(t-1)*nbVarU;
% N = blkdiag(B(:,:,t-1), N);
N = blkdiag(N, B(:,:,t-1)); %To be checked!!!
Su(id1,id2) = M * N;
M = [A(:,:,t-1) * M, A(:,:,t-1)]; %Also M = [A^(n-1), M] or M = [Sx(id1,:), M]
Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
for t=1:nbData-1
id1 = (t-1)*nbVarX+1:t*nbVarX;
id2 = t*nbVarX+1:(t+1)*nbVarX;
id3 = (t-1)*nbVarU+1:t*nbVarU;
Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
Su(id2,id3) = B(:,:,t);
end
end
\ No newline at end of file
......@@ -138,12 +138,12 @@ for t=nbData-1:-1:1
end
%Reproduction with feedback (FB) and feedforward (FF) terms
X = zeros(nbVar,1);
x = zeros(nbVar,1);
rx(:,1) = X; %Log data
for t=2:nbData
u = -K(:,:,t-1) * X + Kff(:,:,t-1) * v(:,t); %Acceleration command with FB and FF terms
X = A * X + B * u; %Update of state vector
rx(:,t) = X; %Log data
u = -K(:,:,t-1) * x + Kff(:,:,t-1) * v(:,t); %Acceleration command with FB and FF terms
x = A * x + B * u; %Update of state vector
rx(:,t) = x; %Log data
end
......
......@@ -183,7 +183,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
figure('position',[10 10 1000 1000],'color',[1 1 1]); axis off; hold on;
plotGMM(model0.Mu(1:2,:), model0.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
......@@ -193,7 +193,6 @@ for n=1:nbRepros
h(2) = plot(r2(n).Data(1,:), r2(n).Data(2,:), '--','linewidth',2,'color',[0 .8 0]);
end
axis equal;
xlabel('x_1'); ylabel('x_2');
legend(h,'LQR with augmented state space',' LQT with FB and FF terms');
% %Plot velocity
......
......@@ -96,7 +96,7 @@ end
figure('position',[10 10 700 700],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
for t=round(linspace(1,nbData,2))
colTmp = [.9,.9,.9] - [.7,.7,.7] * t/nbData;
plotArm(r(1).q(:,t), ones(nbDOFs,1)*armLength, [0;0;-10+t*0.1], .02, colTmp);
plotArm(r(1).q(:,t), ones(nbDOFs,1)*armLength, [0;0;-10+t*0.1], .06, colTmp);
end
% plotArm(model.Mu, ones(nbDOFs,1)*armLength, [0;0;10], .02, [.8 0 0]);
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Gdp_vecTransp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Hd_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Hd_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S1_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S1_interp02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR03
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR04
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GaussProd01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_TPGMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_TPGMM02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -5,7 +5,7 @@ function demo_Riemannian_S2_batchLQR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -5,7 +5,7 @@ function demo_Riemannian_S2_batchLQR02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -5,7 +5,7 @@ function demo_Riemannian_S2_batchLQR03
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR_Bezier01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_infHorLQR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_vecTransp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S3_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S3_GMR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",