Commit 377af0f6 authored by Sylvain CALINON's avatar Sylvain CALINON

timeline plots added

parent 9b9cf0a1
......@@ -35,7 +35,7 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.dt = 1E-2; %Time step size
model.nbData = 100; %Number of datapoints
model.nbIter = 10; %Number of iterations for iLQR
model.nbIter = 20; %Number of iterations for iLQR
model.nbPoints = 2; %Number of viapoints
model.nbVarX = 3; %State space dimension (q1,q2,q3)
model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
......@@ -65,9 +65,13 @@ idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
u = zeros(model.nbVarU*(model.nbData-1), 1);
x0 = [3*pi/4; -pi/2; -pi/4];
A = repmat(eye(model.nbVarX), [1 1 model.nbData-1]);
B = repmat(eye(model.nbVarX, model.nbVarU) * model.dt, [1 1 model.nbData-1]);
[Su0, Sx0] = transferMatrices(A, B); %Constant Su and Sx for the proposed system
%Transfer matrices (for linear system as single integrator)
% A = repmat(eye(model.nbVarX), [1 1 model.nbData-1]);
% B = repmat(eye(model.nbVarX, model.nbVarU) * model.dt, [1 1 model.nbData-1]);
% [Su0, Sx0] = transferMatrices(A, B); %Constant Su and Sx for the proposed system
Su0 = [zeros(model.nbVarX, model.nbVarX*(model.nbData-1)); tril(kron(ones(model.nbData-1), eye(model.nbVarX)*model.dt))];
Sx0 = kron(ones(model.nbData,1), eye(model.nbVarX));
idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
Su = Su0(idx,:);
for n=1:model.nbIter
......@@ -89,7 +93,7 @@ for n=1:model.nbIter
end
alpha = alpha * 0.5;
end
u = u + du * alpha; %Update control by following gradient
u = u + du * alpha;
end
%Log data
......@@ -147,22 +151,22 @@ pause;
close all;
end
%%%%%%%%%%%%%%%%%%%%%%
function [Su, Sx] = transferMatrices(A, B)
[nbVarX, nbVarU, nbData] = size(B);
nbData = nbData+1;
% Sx = kron(ones(nbData,1), speye(nbVarX));
Sx = speye(nbData*nbVarX, nbVarX);
Su = sparse(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
% %%%%%%%%%%%%%%%%%%%%%%
% function [Su, Sx] = transferMatrices(A, B)
% [nbVarX, nbVarU, nbData] = size(B);
% nbData = nbData+1;
% % Sx = kron(ones(nbData,1), speye(nbVarX));
% Sx = speye(nbData*nbVarX, nbVarX);
% Su = sparse(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
%%%%%%%%%%%%%%%%%%%%%%
%Logarithmic map for R^2 x S^1 manifold
......@@ -187,6 +191,7 @@ function f = fkine(x, model)
f = logmap(fkine0(x, model), model.Mu); %Error by considering manifold
for t=1:size(x,2)
% f(:,t) = logmap(fkine0(x(:,t), model), model.Mu(:,t));
f(1:2,t) = model.A(:,:,t)' * f(1:2,t); %Object-centered FK
end
......@@ -227,6 +232,7 @@ function J = jacob(x, f, model)
% Jtmp(i,:) = 0;
% end
% end
J = blkdiag(J, Jtmp);
end
end
\ No newline at end of file
......@@ -35,13 +35,13 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 200; %Number of datapoints
nbPoints = 3; %Number of viapoints
nbData = 100; %Number of datapoints
nbPoints = 2; %Number of viapoints
nbVarPos = 2; %Dimension of position data (here: x1,x2)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbDeriv = 1; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
dt = 1E-2; %Time step duration
rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
dt = 1E-1; %Time step duration
rfactor = 1E-3; %dt^nbDeriv; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor; %Control cost matrix
......@@ -71,6 +71,10 @@ for n=2:nbData
M = [A*M(:,1:nbVarPos), M];
end
% %Build Sx and Su transfer matrices (for nbDeriv=1)
% Su = [zeros(nbVar, nbVar*(nbData-1)); tril(kron(ones(nbData-1), eye(nbVar)*dt))];
% Sx = kron(ones(nbData,1), eye(nbVar));
%% Task setting (viapoints passing)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
......@@ -78,18 +82,28 @@ tl = linspace(1, nbData, nbPoints+1);
tl = round(tl(2:end));
MuQ = zeros(nbVar*nbData,1);
Q = zeros(nbVar*nbData);
%Position tracking
for t=1:length(tl)
id(:,t) = [1:nbVarPos] + (tl(t)-1) * nbVar;
Q(id(:,t), id(:,t)) = eye(nbVarPos);
MuQ(id(:,t)) = rand(nbVarPos,1) - 0.5;
end
% %Position+velocity tracking
% for t=1:length(tl)
% id(:,t) = [1:nbVar] + (tl(t)-1) * nbVar;
% Q(id(:,t), id(:,t)) = eye(nbVar);
% MuQ(id(:,t)) = [rand(nbVarPos,1) - 0.5; zeros(nbVar-nbVarPos,1)];
% end
%% Batch LQR reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0 = zeros(nbVar,1);
u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
u = reshape(u, 2, nbData-1); %Reshape data for plotting
uSigma = inv(Su' * Q * Su + R); % + eye((nbData-1)*nbVarU) * 1E-10;
xSigma = Su * uSigma * Su';
......@@ -97,15 +111,40 @@ xSigma = Su * uSigma * Su';
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 800 800]); hold on; axis off;
for t=1:nbData
plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-6, [.2 .2 .2], .1);
end
figure('position',[10 10 600 600]); hold on; axis off;
% for t=1:nbData
% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-6, [.2 .2 .2], .1);
% 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]);
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',35,'color',[.8 0 0]);
axis equal;
% print('-dpng','graphs/demo_MPC01.png');
% print('-dpng','graphs/demo_LQT01.png');
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbVar = min(nbVar,4);
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$'};
figure('position',[620 10 600 850],'color',[1 1 1]);
%State profile
for j=1:nbVar
subplot(nbVar+nbVarPos+1,1,j); hold on;
plot(rx(j,:), '-','linewidth',2,'color',[0 0 0]);
if j<=nbVarPos
plot(tl, MuQ(id(j,:)), '.','markersize',40,'color',[.8 0 0]);
end
axis([1, nbData, min(rx(j,:))-1E-3, max(rx(j,:))+1E-3]);
ylabel(labList{j},'fontsize',18,'interpreter','latex');
end
%Control profile
for j=1:nbVarPos
subplot(nbVar+nbVarPos+1,1,nbVar+1+j); hold on;
plot(u(j,:), '-','linewidth',2,'color',[0 0 0]);
axis([1, nbData-1, min(u(j,:))-1E-6, max(u(j,:))+1E-6]);
ylabel(['$u_' num2str(j) '$'],'fontsize',18,'interpreter','latex');
end
xlabel('$t$','fontsize',18,'interpreter','latex');
pause;
close all;
\ No newline at end of file
......@@ -40,9 +40,9 @@ nbRepros = 1; %Number of reproductions in new situations
nbNewRepros = 10; %Number of stochastically generated reproductions
nbData = 200; %Number of datapoints
model.nbStates = 3; %Number of Gaussians in the GMM
model.nbStates = 1; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
model.nbDeriv = 1; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
model.dt = 1E-2; %Time step duration
model.rfactor = 1E-3; %model.dt^model.nbDeriv; %Control cost in LQR
......@@ -436,7 +436,7 @@ for j=1:model.nbVarPos
end
xlabel('$t$','fontsize',14,'interpreter','latex');
%print('-dpng','graphs/demo_batchLQR02.png');
%print('-dpng','graphs/demo_LQT02.png');
%% Plot covariance of control trajectory distribution
......
function demo_MPC_LQT04
function demo_OC_LQT04
% Control of a spring attached to a point with batch LQR (with augmented state space)
%
% If this code is useful for your research, please cite the related publication:
......
......@@ -44,7 +44,7 @@ sp = (nbVar + 1) / 2; %Sobolev norm parameter
dt = 1E-2; %Time step
xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
om = 2 * pi / L; %omega
om = 2 * pi / L; %omega parameter
u_max = 1E1; %Maximum speed allowed
%Desired spatial distribution represented as a mixture of Gaussians
......@@ -54,14 +54,14 @@ u_max = 1E1; %Maximum speed allowed
% Sigma(:,:,2) = [.1;.2]*[.1;.2]' .*8E-1 + eye(nbVar)*1E-3;
Mu(:,1) = [.5; .7];
Sigma(:,:,1) = [.3;.1]*[.3;.1]' .*5E-1 + eye(nbVar)*5E-3; %eye(nbVar).*1E-2;
Sigma(:,:,1) = [.3;.1]*[.3;.1]' *5E-1 + eye(nbVar)*5E-3; %eye(nbVar).*1E-2;
Mu(:,2) = [.6; .3];
Sigma(:,:,2) = [.1;.2]*[.1;.2]' .*3E-1 + eye(nbVar)*1E-2;
Sigma(:,:,2) = [.1;.2]*[.1;.2]' *3E-1 + eye(nbVar)*1E-2;
% Mu = rand(nbVar, nbStates);
% Sigma = repmat(eye(nbVar)*1E-1, [1,1,nbStates]);
Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
Priors = ones(1,nbStates) / nbStates; %Mixing coefficients
%% Compute Fourier series coefficients phi_k of desired spatial distribution
......@@ -82,7 +82,7 @@ for j=1:nbStates
for n=1:size(op,2)
MuTmp = diag(op(:,n)) * Mu(:,j); %Eq.(20)
SigmaTmp = diag(op(:,n)) * Sigma(:,:,j) * diag(op(:,n))'; %Eq.(20)
w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
w_hat = w_hat + Priors(j) * cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
end
end
w_hat = w_hat / L^nbVar / size(op,2);
......@@ -94,9 +94,9 @@ w_hat = w_hat / L^nbVar / size(op,2);
% [xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
% g = zeros(1,nbRes^nbVar);
% for k=1:nbStates
% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
% g = g + Priors(k) * mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
% end
% phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) ./ L^nbVar ./ nbRes^nbVar;
% phi_inv = cos(KX(1,:)' * xm(1,:) * om) .* cos(KX(2,:)' * xm(2,:) * om) / L^nbVar / nbRes^nbVar;
% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
......@@ -104,11 +104,11 @@ w_hat = w_hat / L^nbVar / size(op,2);
nbRes = 100;
xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
[xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* 2^nbVar; %Fourier basis functions
% % phim(2:end,:) = phim(2:end,:) .* 2;
phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) * om) * 2^nbVar; %Fourier basis functions
% % phim(2:end,:) = phim(2:end,:) * 2;
% phim = phim .* 2^nbVar;
% phim(1:nbFct,:) = phim(1:nbFct,:) .* .5;
% phim(1:nbFct:end,:) = phim(1:nbFct:end,:) .* .5;
% phim(1:nbFct,:) = phim(1:nbFct,:) * .5;
% phim(1:nbFct:end,:) = phim(1:nbFct:end,:) * .5;
hk = [1; 2*ones(nbFct-1,1)];
HK = hk(xx(:)) .* hk(yy(:));
phim = phim .* repmat(HK,[1,nbRes^nbVar]);
......@@ -119,7 +119,7 @@ g = w_hat' * phim;
% %Alternative computation of g
% g = zeros(1,nbRes^nbVar);
% for k=1:nbStates
% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
% g = g + Priors(k) * mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
% end
......@@ -131,22 +131,22 @@ for t=1:nbData
r.x(:,t) = x; %Log data
%Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction)
phi1 = cos(x * rg .* om); %Eq.(18)
dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
phi1 = cos(x * rg * om); %Eq.(18)
dphi1 = -sin(x * rg * om) .* repmat(rg,nbVar,1) * om;
dphi = [dphi1(1,xx) .* phi1(2,yy); phi1(1,xx) .* dphi1(2,yy)]; %Gradient of basis functions
wt = wt + (phi1(1,xx) .* phi1(2,yy))' ./ L.^nbVar; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
wt = wt + (phi1(1,xx) .* phi1(2,yy))' / L^nbVar; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
% u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 5E-1; %Velocity command
%Controller with constrained velocity norm
u = -dphi * (Lambda .* (wt./t - w_hat)); %Eq.(24)
u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
u = -dphi * (Lambda .* (wt/t - w_hat)); %Eq.(24)
u = u * u_max / (norm(u)+1E-1); %Velocity command
x = x + u .* dt; %Update of position
r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution (for visualization)
r.w(:,t) = wt./t; %Fourier coefficients along trajectory (for visualization)
x = x + u * dt; %Update of position
r.g(:,t) = (wt/t)' * phim; %Reconstructed spatial distribution (for visualization)
r.w(:,t) = wt/t; %Fourier coefficients along trajectory (for visualization)
% r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
end
......@@ -168,11 +168,10 @@ colormap(repmat(linspace(1,.4,64),3,1)');
%x
subplot(1,3,1); hold on; axis off;
colormap(repmat(linspace(1,.4,64),3,1)');
G = reshape(g,[nbRes,nbRes]);
% G = reshape(r.g(:,end),[nbRes,nbRes]);
G([1,end],:) = max(g);
G(:,[1,end]) = max(g);
G([1,end],:) = max(g); %Add vertical image borders
G(:,[1,end]) = max(g); %Add horizontal image borders
surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), G, 'FaceColor','interp','EdgeColor','interp');
% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
% plotGMM(Mu, Sigma, [.2 .2 .2], .3);
......
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