diff --git a/demos/demo_TPGMM01.m b/demos/demo_TPGMM01.m
index 5602081a3a90633175312cff9beea26c36b2e15e..5384b13b297b964b8080e0f1c4785d93e2f4331d 100644
--- a/demos/demo_TPGMM01.m
+++ b/demos/demo_TPGMM01.m
@@ -93,20 +93,19 @@ end
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,2200,800]);
+figure('position',[10,10,2300,900]);
 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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %DEMOS
 subplot(1,model.nbFrames+1,1); hold on; box on; title('Demonstrations');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(s(n).Data(2,1), s(n).Data(3,1),'.','markersize',15,'color',clrmap(n,:));
@@ -116,7 +115,9 @@ for n=1:nbSamples
 end
 axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
-%FRAMES
+%MODEL
+p0.A = eye(2);
+p0.b = zeros(2,1);
 for m=1:model.nbFrames
 	subplot(1,model.nbFrames+1,1+m); hold on; grid on; box on; title(['Frame ' num2str(m)]);
 	for n=1:nbSamples
@@ -126,7 +127,8 @@ for m=1:model.nbFrames
 			squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
 	end
 	plotGMM(squeeze(model.Mu(:,m,:)), squeeze(model.Sigma(:,:,m,:)), [.5 .5 .5],.8);
-	axis square; set(gca,'xtick',[0],'ytick',[0]);
+	plotPegs(p0, colPegs(m,:));
+	axis equal; axis([-4.5 4.5 -1 8]); set(gca,'xtick',[0],'ytick',[0]);
 end
 %print('-dpng','graphs/demo_TPGMM01.png');
 
@@ -154,4 +156,21 @@ end
 % end
 
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A * pegMesh + repmat(p(m).b,1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_TPGMR01.m b/demos/demo_TPGMR01.m
index 155ab9a1ee78e4dd9f64d1d20c2d36373e6114ff..509a26b27261dc13cf19567a388bc9bde914890f 100644
--- a/demos/demo_TPGMR01.m
+++ b/demos/demo_TPGMR01.m
@@ -159,20 +159,19 @@ end
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,2300,900]);
+figure('position',[10,10,2300,900]);
 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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %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]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(s(n).Data(2,1), s(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
@@ -185,8 +184,7 @@ subplot(1,3,2); hold on; box on; title('Reproductions with GMR');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([r(n).p(m).b(2) r(n).p(m).b(2)+r(n).p(m).A(2,3)], [r(n).p(m).b(3) r(n).p(m).b(3)+r(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(r(n).p(m).b(2), r(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(r(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbSamples
@@ -205,8 +203,7 @@ subplot(1,3,3); hold on; box on; title('New reproductions with GMR');
 for n=1:nbRepros
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([r(nbSamples+n).p(m).b(2) r(nbSamples+n).p(m).b(2)+r(nbSamples+n).p(m).A(2,3)], [r(nbSamples+n).p(m).b(3) r(nbSamples+n).p(m).b(3)+r(nbSamples+n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(r(nbSamples+n).p(m).b(2), r(nbSamples+n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(r(nbSamples+n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbRepros
@@ -255,4 +252,21 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
 % print('-dpng','graphs/demo_TPGMR01.png');
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_TPGP01.m b/demos/demo_TPGP01.m
index 1ad32f2468f53697c3636c4b6b0f8425ec1bbc68..7ba31fb1ad5be1a32403ca1e176879ad160c5214 100644
--- a/demos/demo_TPGP01.m
+++ b/demos/demo_TPGP01.m
@@ -93,20 +93,19 @@ end
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,2300,900]);
+figure('position',[10,10,2300,900]);
 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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
@@ -119,8 +118,7 @@ subplot(1,3,2); hold on; box on; title('Reproductions');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbSamples
@@ -135,8 +133,7 @@ subplot(1,3,3); hold on; box on; title('New reproductions');
 for n=1:nbRepros
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(rnew(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbRepros
@@ -148,4 +145,21 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
 %print('-dpng','graphs/demo_TPGP01.png');
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A * pegMesh + repmat(p(m).b,1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_TPMPC01.m b/demos/demo_TPMPC01.m
index 64336c89c06ca12df9a094e7882588fad66cb12c..0bb50a12752c45f3feb6d23d76e7989b4619b8ce 100644
--- a/demos/demo_TPMPC01.m
+++ b/demos/demo_TPMPC01.m
@@ -228,15 +228,14 @@ 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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %Demonstrations
 subplot(1,5,1); hold on; box on; title('Demonstrations');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:));
@@ -249,8 +248,7 @@ subplot(1,5,2); hold on; box on; title('Reproductions');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbSamples
@@ -265,8 +263,7 @@ subplot(1,5,3); hold on; box on; title('Reproductions in new situations');
 for n=1:nbRepros
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(rnew(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbRepros
@@ -277,6 +274,8 @@ end
 axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
 %Model
+p0.A = eye(2);
+p0.b = zeros(2,1);
 for m=1:model.nbFrames
 	subplot(1,5,3+m); hold on; grid on; box on; title(['Model - Frame ' num2str(m)]);
 	for n=1:nbSamples
@@ -284,7 +283,8 @@ for m=1:model.nbFrames
 		plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
 	end
 	plotGMM(squeeze(model.Mu(1:2,m,:)), squeeze(model.Sigma(1:2,1:2,m,:)+eye(2).*1E-2), [.5 .5 .5], .4);
-	axis square; set(gca,'xtick',[0],'ytick',[0]);
+	plotPegs(p0, colPegs(m,:));
+	axis equal; axis([-4.5 4.5 -1 8]); set(gca,'xtick',[0],'ytick',[0]);
 end
 
 %Timeline plot
@@ -326,4 +326,21 @@ for m=1:model.nbFrames
 end
 
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_TPproMP01.m b/demos/demo_TPproMP01.m
index 14bf8a08c9a75487da8e4996f8068c0a34c5779b..8a8d2394f1c46b5cb9dfb3b416501b394376527a 100644
--- a/demos/demo_TPproMP01.m
+++ b/demos/demo_TPproMP01.m
@@ -148,20 +148,19 @@ end
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,2300,500]);
+figure('position',[10,10,2300,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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %DEMO
 subplot(1,model.nbFrames+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]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot demonstrations
 	plot(s(n).Data(2,1), s(n).Data(3,1),'.','markersize',15,'color',clrmap(n,:));
@@ -174,8 +173,7 @@ subplot(1,model.nbFrames+3,2); hold on; box on; title('Reproductions');
 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]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot Gaussians
 	plotGMM(s(n).Mu(2:3,:), s(n).Sigma(2:3,2:3,:), [.5 .5 .5], .3);
@@ -189,8 +187,7 @@ subplot(1,model.nbFrames+3,3); hold on; box on; title('New reproductions');
 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]);
+		plotPegs(rnew(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(rnew(n).MuTraj(2), rnew(n).MuTraj(3), '.','markersize',12,'color',clrmap(n,:));
@@ -198,7 +195,9 @@ for n=1:nbRepros
 end
 axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
-%FRAMES
+%MODEL
+p0.A = eye(3);
+p0.b = zeros(3,1);
 for m=1:model.nbFrames
 	subplot(1,model.nbFrames+3,3+m); hold on; grid on; box on; title(['Frame ' num2str(m)]);
 	for n=1:nbSamples
@@ -206,7 +205,8 @@ for m=1:model.nbFrames
 		plot(squeeze(Data(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), squeeze(Data(3,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
 	end
 	plotGMM(squeeze(model.Mu2(2:end,m,:)), squeeze(model.Sigma2(2:end,2:end,m,:)), [.5 .5 .5], .3);
-	axis square; set(gca,'xtick',[0],'ytick',[0]);
+	plotPegs(p0, colPegs(m,:));
+	axis equal; axis([-4.5 4.5 -1 8]); set(gca,'xtick',[0],'ytick',[0]);
 end
 
 % %Reproductions in same situations
@@ -241,4 +241,21 @@ end
 
 % print('-dpng','graphs/demo_TPproMP01.png');
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_TPtrajDistrib01.m b/demos/demo_TPtrajDistrib01.m
index e57e311e5ddd9e137afae329307b3ec5789cf385..75e37e28e617e15105c50d308b32cb9afe082116 100644
--- a/demos/demo_TPtrajDistrib01.m
+++ b/demos/demo_TPtrajDistrib01.m
@@ -127,20 +127,19 @@ end
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,2300,900]);
+figure('position',[10,10,2300,900]);
 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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
@@ -153,8 +152,7 @@ subplot(1,3,2); hold on; box on; title('Reproductions');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbSamples
@@ -169,8 +167,7 @@ subplot(1,3,3); hold on; box on; title('New reproductions');
 for n=1:nbRepros
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(rnew(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbRepros
@@ -212,4 +209,21 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
 %print('-dpng','graphs/demo_TPtrajDistrib01.png');
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A * pegMesh + repmat(p(m).b,1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_TPtrajGMM01.m b/demos/demo_TPtrajGMM01.m
index ce2d0d6cd8f8c6b6b321a3f338389566174f4360..39bd1689a8d486b21f6a094c7303f8f188b8ec48 100644
--- a/demos/demo_TPtrajGMM01.m
+++ b/demos/demo_TPtrajGMM01.m
@@ -183,20 +183,19 @@ end
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,2300,900]);
+figure('position',[10,10,2300,900]);
 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]];
+colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
 
 %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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 	%Plot trajectories
 	plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:));
@@ -209,8 +208,7 @@ subplot(1,3,2); hold on; box on; title('Reproductions');
 for n=1:nbSamples
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(s(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbSamples
@@ -229,8 +227,7 @@ subplot(1,3,3); hold on; box on; title('New reproductions');
 for n=1:nbRepros
 	%Plot frames
 	for m=1:model.nbFrames
-		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
-		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+		plotPegs(rnew(n).p(m), colPegs(m,:));
 	end
 end
 for n=1:nbRepros
@@ -246,4 +243,21 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
 %print('-dpng','graphs/demo_TPtrajGMM01.png');
 pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%Function to plot pegs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function h = plotPegs(p, colPegs, fa)
+	if ~exist('colPegs')
+		colPegs = [0.2863    0.0392    0.2392; 0.9137    0.4980    0.0078];
+	end
+	if ~exist('fa')
+		fa = .6;
+	end
+	pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
+	for m=1:length(p)
+		dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
+		h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
+	end
+end
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m
index eb4a070ffd81bc23ce3b32301df99711354fe394..97a35ebda855da9b40fad5918f447b184be9c02e 100644
--- a/demos/demo_ergodicControl_1D01.m
+++ b/demos/demo_ergodicControl_1D01.m
@@ -4,12 +4,14 @@ function demo_ergodicControl_1D01
 %
 % If this code is useful for your research, please cite the related publication:
 % @incollection{Calinon19MM,
-%   author="Calinon, S.",
-%   title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-%   booktitle="Mixture Models and Applications",
-%   publisher="Springer",
-%   editor="Bouguila, N. and Fan, W.", 
-%   year="2019"
+% 	author="Calinon, S.",
+% 	title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
+% 	booktitle="Mixture Models and Applications",
+% 	publisher="Springer",
+% 	editor="Bouguila, N. and Fan, W.", 
+% 	year="2019",
+% 	pages="39--57",
+% 	doi="10.1007/978-3-030-23876-6_3"
 % }
 %
 % Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index 14373f12a0bdf905f419d837a188d4c61df51a97..b1cd448f40b0348dee7da56efc8a591eb0d59074 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -4,12 +4,14 @@ function demo_ergodicControl_2D01
 %
 % If this code is useful for your research, please cite the related publication:
 % @incollection{Calinon19MM,
-%   author="Calinon, S.",
-%   title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-%   booktitle="Mixture Models and Applications",
-%   publisher="Springer",
-%   editor="Bouguila, N. and Fan, W.", 
-%   year="2019"
+% 	author="Calinon, S.",
+% 	title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
+% 	booktitle="Mixture Models and Applications",
+% 	publisher="Springer",
+% 	editor="Bouguila, N. and Fan, W.", 
+% 	year="2019",
+% 	pages="39--57",
+% 	doi="10.1007/978-3-030-23876-6_3"
 % }
 %
 % Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_ergodicControl_3D01.m b/demos/demo_ergodicControl_3D01.m
index 4ef0d6ba0afbc93deb879a745b8fc026971d19bd..3c9d788b7ddc15ffd9a4d11b9c19b38302ac94b3 100644
--- a/demos/demo_ergodicControl_3D01.m
+++ b/demos/demo_ergodicControl_3D01.m
@@ -4,12 +4,14 @@ function demo_ergodicControl_3D01
 %
 % If this code is useful for your research, please cite the related publication:
 % @incollection{Calinon19MM,
-%   author="Calinon, S.",
-%   title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-%   booktitle="Mixture Models and Applications",
-%   publisher="Springer",
-%   editor="Bouguila, N. and Fan, W.", 
-%   year="2019"
+% 	author="Calinon, S.",
+% 	title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
+% 	booktitle="Mixture Models and Applications",
+% 	publisher="Springer",
+% 	editor="Bouguila, N. and Fan, W.", 
+% 	year="2019",
+% 	pages="39--57",
+% 	doi="10.1007/978-3-030-23876-6_3"
 % }
 %
 % Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
diff --git a/demos/demo_ergodicControl_nD01.m b/demos/demo_ergodicControl_nD01.m
index db065da003654c5224e7c7f6462620156c887b60..bcca83d6ebabb944145d77dc85f9dbbd0b19f6e9 100644
--- a/demos/demo_ergodicControl_nD01.m
+++ b/demos/demo_ergodicControl_nD01.m
@@ -4,12 +4,14 @@ function demo_ergodicControl_nD01
 %
 % If this code is useful for your research, please cite the related publication:
 % @incollection{Calinon19MM,
-%   author="Calinon, S.",
-%   title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-%   booktitle="Mixture Models and Applications",
-%   publisher="Springer",
-%   editor="Bouguila, N. and Fan, W.", 
-%   year="2019"
+% 	author="Calinon, S.",
+% 	title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
+% 	booktitle="Mixture Models and Applications",
+% 	publisher="Springer",
+% 	editor="Bouguila, N. and Fan, W.", 
+% 	year="2019",
+% 	pages="39--57",
+% 	doi="10.1007/978-3-030-23876-6_3"
 % }
 %
 % Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/