From 377af0f6f46294ebdfb9d1877d7a6d612187d1cf Mon Sep 17 00:00:00 2001
From: scalinon <sylvain.calinon@idiap.ch>
Date: Tue, 16 Mar 2021 23:36:15 +0100
Subject: [PATCH] timeline plots added

---
 demos/demo_OC_DDP_manipulator01.m | 48 ++++++++++++++-----------
 demos/demo_OC_LQT01.m             | 59 +++++++++++++++++++++++++------
 demos/demo_OC_LQT02.m             |  6 ++--
 demos/demo_OC_LQT04.m             |  2 +-
 demos/demo_ergodicControl_2D01.m  | 45 ++++++++++++-----------
 5 files changed, 102 insertions(+), 58 deletions(-)

diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index d3d8ccc..30e607b 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -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
diff --git a/demos/demo_OC_LQT01.m b/demos/demo_OC_LQT01.m
index 6a06826..12e030f 100644
--- a/demos/demo_OC_LQT01.m
+++ b/demos/demo_OC_LQT01.m
@@ -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
diff --git a/demos/demo_OC_LQT02.m b/demos/demo_OC_LQT02.m
index 837ddfd..e2acd13 100644
--- a/demos/demo_OC_LQT02.m
+++ b/demos/demo_OC_LQT02.m
@@ -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
diff --git a/demos/demo_OC_LQT04.m b/demos/demo_OC_LQT04.m
index 7a7a646..86bb508 100644
--- a/demos/demo_OC_LQT04.m
+++ b/demos/demo_OC_LQT04.m
@@ -1,4 +1,4 @@
-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:
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index 4bbad65..1b2223c 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -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);
-- 
GitLab