New demo_affineTransform01.m example

README.md

@@ -55,6 +55,7 @@ All the examples are located in the main folder, and the functions are located i
 | benchmark_DS_TP_LWR01 | Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) |
 | benchmark_DS_TP_MFA01 | Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction |
 | benchmark_DS_TP_trajGMM01 | Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction |
+| demo_affineTransform01 | Miscellaneous affine transformations of raw data as pre-processing step to train a task-parameterized model |
 | demo_batchLQR01 | Controller retrieval through a batch solution of linear quadratic optimal control (unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_iterativeLQR01) |
 | demo_DSGMR01 | Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, with decay variable used as input (as in DMP) |
 | demo_DTW01 | Trajectory realignment through dynamic time warping (DTW) |

demo_affineTransform01.m

+function demo_affineTransform01
+% Miscellaneous affine transformations of raw data as pre-processing step to train a task-parameterized model 
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% This source code is given for free! In exchange, I would be grateful if you cite
+% the following reference in any academic publication that uses this code or part of it:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2)
+model.dt = 0.01; %Time step
+nbData = 200; %Number of datapoints in a trajectory
+
+
+%% Load 3rd order tensor data (for [x] encoding)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=2 the dimension of a
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
+load('data/Data01.mat');
+
+
+%% Task parameters and observed data for [t,x] encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model2.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+%Create 3rd order tensor data and task parameters for [t,x]
+Data2 = zeros(model2.nbVar, model.nbFrames, nbSamples*nbData);
+for n=1:nbSamples
+	%size(s(n).Data)
+	for m=1:model.nbFrames
+		s2(n).p(m).b = [0; s(n).p(m).b];
+		s2(n).p(m).A = eye(model2.nbVar);
+		s2(n).p(m).A(2:end,2:end) = s(n).p(m).A;
+		Data2(:,m,(n-1)*nbData+1:n*nbData) = s2(n).p(m).A \ (s(n).Data0 - repmat(s2(n).p(m).b, 1, nbData));
+	end
+end
+
+
+%% Task parameters and observed data for [x,dx] encoding
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model3.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model3.nbDeriv = 2; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
+model3.nbVar = model3.nbVarPos * model3.nbDeriv; %Dimension of state vector
+%Create transformation matrix to compute derivatives
+D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt;
+D(end,end) = 0;
+%Create 3rd order tensor data and task parameters for [x,dx]
+Data3 = zeros(model3.nbVar, model.nbFrames, nbSamples*nbData);
+for n=1:nbSamples
+	s3(n).Data = zeros(model3.nbVar, model.nbFrames, nbData);
+	s3(n).Data0 = s(n).Data0(2:end,:); %Remove time
+	DataTmp = s3(n).Data0;
+	for k=1:model3.nbDeriv-1
+		DataTmp = [DataTmp; s3(n).Data0*D^k]; %Compute derivatives
+	end
+	for m=1:model.nbFrames
+		s3(n).p(m).b = [s(n).p(m).b; zeros((model3.nbDeriv-1)*model3.nbVarPos,1)];
+		s3(n).p(m).A = kron(eye(model3.nbDeriv), s(n).p(m).A);
+		s3(n).Data(:,m,:) = s3(n).p(m).A \ (DataTmp - repmat(s3(n).p(m).b, 1, nbData));
+		Data3(:,m,(n-1)*nbData+1:n*nbData) = s3(n).Data(:,m,:);
+	end
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,700]);
+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]];
+
+%DEMOS1
+subplot(3,model.nbFrames+1,1); hold on; box on; title('Demonstrations [x]');
+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]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%DEMOS2
+subplot(3,model.nbFrames+1,4); hold on; box on; title('Demonstrations [t,x]');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s2(n).p(m).b(2) s2(n).p(m).b(2)+s2(n).p(m).A(2,3)], [s2(n).p(m).b(3) s2(n).p(m).b(3)+s2(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s2(n).p(m).b(2), s2(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%DEMOS3
+subplot(3,model.nbFrames+1,7); hold on; box on; title('Demonstrations [x,dx]');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s3(n).p(m).b(1) s3(n).p(m).b(1)+s3(n).p(m).A(1,2)], [s3(n).p(m).b(2) s3(n).p(m).b(2)+s3(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s3(n).p(m).b(1), s3(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%FRAMES
+for m=1:model.nbFrames
+	subplot(3,model.nbFrames+1,1+m); hold on; grid on; box on; title(['Frame ' num2str(m) ' [x]']);
+	for n=1:nbSamples
+		plot(squeeze(Data(1,m,(n-1)*s(1).nbData+1)), ...
+			squeeze(Data(2,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+		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
+	axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%FRAMES2
+for m=1:model.nbFrames
+	subplot(3,model.nbFrames+1,4+m); hold on; grid on; box on; title(['Frame ' num2str(m) ' [t,x]']);
+	for n=1:nbSamples
+		plot(squeeze(Data2(2,m,(n-1)*s(1).nbData+1)), ...
+			squeeze(Data2(3,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+		plot(squeeze(Data2(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), ...
+			squeeze(Data2(3,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
+	end
+	axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%FRAMES3
+for m=1:model.nbFrames
+	subplot(3,model.nbFrames+1,7+m); hold on; grid on; box on; title(['Frame ' num2str(m) ' [x,dx]']);
+	for n=1:nbSamples
+		plot(squeeze(Data3(1,m,(n-1)*s(1).nbData+1)), ...
+			squeeze(Data3(2,m,(n-1)*s(1).nbData+1)), '.','markersize',15,'color',clrmap(n,:));
+		plot(squeeze(Data3(1,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), ...
+			squeeze(Data3(2,m,(n-1)*s(1).nbData+1:n*s(1).nbData)), '-','linewidth',1.5,'color',clrmap(n,:));
+	end
+	axis square; set(gca,'xtick',[0],'ytick',[0]);
+end
+
+%print('-dpng','graphs/demo_affineTransform01.png');
+%pause;
+%close all;