Commit 2c3d6d31 authored by Sylvain Calinon's avatar Sylvain Calinon

New examples added

parent 506647eb
......@@ -8,32 +8,61 @@ The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, Idiap
Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used parts of these codes.
* Tutorial (covers GMM, TP-GMM, MFA, MPPCA, GMR, LWR, GPR, MPC, LQR, trajGMM):
```
@article{Calinon16JIST,
author="Calinon, S.",
title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
journal="Intelligent Service Robotics",
publisher="Springer Berlin Heidelberg",
issn="1861-2776",
doi="10.1007/s11370-015-0187-9",
year="2016",
volume="9",
number="1",
pages="1--29"
pages="1--29",
doi="10.1007/s11370-015-0187-9",
}
```
* HMM, HSMM:
```
@inproceedings{Calinon14ICRA,
author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
title="A task-parameterized probabilistic model with minimal intervention control",
booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
year="2014",
month="May-June",
address="Hong Kong, China",
pages="3339--3344"
@article{Rozo16Frontiers,
author="Rozo, L. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
title="Learning Controllers for Reactive and Proactive Behaviors in Human-Robot Collaboration",
journal="Frontiers in Robotics and {AI}",
year="2016",
month="June",
volume="3",
number="30",
pages="1--11",
doi="10.3389/frobt.2016.00030"
}
```
* Semi-tied covariances in mixture models:
@article{Tanwani16RAL,
author="Tanwani, A. K. and Calinon, S.",
title="Learning Robot Manipulation Tasks with Task-Parameterized Semi-Tied Hidden Semi-{M}arkov Model",
journal="{IEEE} Robotics and Automation Letters ({RA-L})",
year="2016",
month="January",
volume="1",
number="1",
pages="235--242",
doi="10.1109/LRA.2016.2517825"
}
* DP-means:
@article{Bruno16AURO,
author="Bruno, D. and Calinon, S. and Caldwell, D. G.",
title="Learning Autonomous Behaviours for the Body of a Flexible Surgical Robot",
journal="Autonomous Robots",
year="2016",
volume="",
number="",
pages="",
doi="10.1007/s10514-016-9544-6",
}
### Compatibility
The codes are compatible with both Matlab and GNU Octave.
......@@ -77,13 +106,16 @@ All the examples are located in the main folder, and the functions are located i
| demo_affineTransform01 | 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_batchLQR02 | Same as demo_batchLQR01 but with only position data |
| demo_batchLQR_augmSigma01 | Batch LQR with augmented covariance to transform a tracking problem to a regulation problem |
| demo_DMP01 | Dynamic movement primitive (DMP) encoding with radial basis functions |
| demo_DMP02 | Generalization of dynamic movement primitive (DMP) with polynomial fitting using radial basis functions |
| demo_DMP_GMR01 | Emulation of a standard dynamic movement primitive (DMP) by using a GMM with diagonal covariance matrix, and retrieval computed through Gaussian mixture regression (GMR) |
| demo_DMP_GMR02 | Same as demo_DMP_GMR01 but with full covariance matrices coordinating the different variables |
| demo_DMP_GMR03 | Same as demo_DMP_GMR02 but with GMR used to regenerate the path of a spring-damper system instead of encoding the nonlinear forcing term |
| demo_DMP_GMR04 | Same as demo_DMP_GMR03 by using the task-parameterized model formalism |
| demo_DMP_GMR_LQR01 | Same as demo_DMP_GMR04 but with LQR used to refine the parameters of the spring-damper system |
| demo_DMP_GMR_LQR02 | Same as demo_DMP_GMR_LQR01 with perturbations added to show the benefit of full covariance to coordinate disturbance rejection |
| demo_DPMeans_Online01 | Online clustering with DP-Means algorithm |
| demo_DPMeans_Online01 | Online clustering with DP-means algorithm |
| 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_GMM01 | Gaussian mixture model (GMM) parameters estimation |
......@@ -110,7 +142,7 @@ All the examples are located in the main folder, and the functions are located i
| demo_TPGP01 | Task-parameterized Gaussian process regression (TP-GPR) |
| demo_TPHDDC01 | Task-parameterized high dimensional data clustering (TP-HDDC) |
| demo_TPMFA01 | Task-parameterized mixture of factor analyzers (TP-MFA), without motion retrieval |
|demo_TPMPC01 | Task-parameterized model encoding position data, with MPC used to track the associated stepwise reference path |
| demo_TPMPC01 | Task-parameterized model encoding position data, with MPC used to track the associated stepwise reference path |
| demo_TPMPC02 | Same as demo_TPMPC01 with a generalized version of MPC used to track associated stepwise reference paths in multiple frames |
| demo_TPMPPCA01 | Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) |
| demo_TPtrajGMM01 | Task-parameterized model with trajectory-GMM encoding |
......
function demo_DMP01
% Dynamical movement primitive (DMP) with locally weighted regression (LWR).
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
% journal="Intelligent Service Robotics",
% publisher="Springer Berlin Heidelberg",
% doi="10.1007/s11370-015-0187-9",
% year="2016",
% volume="9",
% number="1",
% pages="1--29"
% }
%
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of activation functions (i.e., number of states in the GMM)
model.nbVar = 1; %Number of variables for the radial basis functions [s] (decay term)
model.nbVarPos = 2; %Number of motion variables [x1,x2]
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
nbData = 200; %Length of each trajectory
nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
posId=[1:model.nbVarPos]; velId=[model.nbVarPos+1:2*model.nbVarPos]; accId=[2*model.nbVarPos+1:3*model.nbVarPos];
demos=[];
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
end
xTar = demos{1}.pos(:,end);
Data=[];
DataDMP=[];
for n=1:nbSamples
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
Data = [Data s(n).Data]; %Concatenation of the multiple demonstrations
%Nonlinear forcing term
DataDMP = [DataDMP, (s(n).Data(accId,:) - ...
(repmat(xTar,1,nbData)-s(n).Data(posId,:))*model.kP + s(n).Data(velId,:)*model.kV) ./ repmat(sIn,model.nbVarPos,1)];
end
%% Setting of the basis functions and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_timeBased(sIn, model);
%model = init_GMM_logBased(sIn, model); %Log-spread in s <-> equal spread in t
%model = init_GMM_kmeans(sIn, model);
%Set Sigma as diagonal matrices (i.e., inpedendent systems synchronized by the s variable)
for i=1:model.nbStates
model.Sigma(:,:,i) = 2E-3; %Setting of covariance
end
%Compute activation
H = zeros(model.nbStates,nbData);
for i=1:model.nbStates
H(i,:) = gaussPDF(sIn, model.Mu(:,i), model.Sigma(:,:,i));
end
H = H ./ repmat(sum(H),model.nbStates,1);
H2 = repmat(H,1,nbSamples);
% %Nonlinear force profile retrieval (as in RBFN)
% MuF = DataDMP * pinv(H2);
%Nonlinear force profile retrieval (as in LWR)
X = ones(nbSamples*nbData,1);
Y = DataDMP';
for i=1:model.nbStates
W = diag(H2(i,:));
MuF(:,i) = (X'*W*X \ X'*W * Y)';
end
%Motion retrieval with DMP
currF = MuF * H;
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
%Compute acceleration, velocity and position
ddx = L * [xTar-x; -dx] + currF(:,t) * sIn(t);
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('PaperPosition',[0 0 16 4],'position',[10,10,1300,500],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Spatial plot
axes('Position',[0 0 .2 1]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).Data(1,:),r(1).Data(2,:),'-','linewidth',3,'color',[.8 0 0]);
axis equal; axis square;
%Timeline plot of the nonlinear perturbing force
axes('Position',[.25 .58 .7 .4]); hold on;
for n=1:nbSamples
plot(sIn, DataDMP(1,(n-1)*nbData+1:n*nbData), '-','linewidth',2,'color',[.7 .7 .7]);
end
[~,id] = max(H);
for i=1:model.nbStates
plot(sIn(id==i), repmat(MuF(1,i),1,sum(id==i)), '-','linewidth',6,'color',min(clrmap(i,:)+0.5,1));
end
plot(sIn, currF(1,:), '-','linewidth',2,'color',[.8 0 0]);
axis([min(sIn) max(sIn) min(DataDMP(1,:)) max(DataDMP(1,:))]);
ylabel('$F_1$','fontsize',16,'interpreter','latex');
view(180,-90);
%Timeline plot of the basis functions activation
axes('Position',[.25 .12 .7 .4]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, H(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, H(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([min(sIn) max(sIn) 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP01.png');
%pause;
%close all;
function demo_DMP02
% Generalization of dynamical movement primitive (DMP) with polynomial fitting using locally weighted regression (LWR).
%
% Writing code takes time. Polishing it and making it available to others takes longer!
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST,
% author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
% journal="Intelligent Service Robotics",
% publisher="Springer Berlin Heidelberg",
% doi="10.1007/s11370-015-0187-9",
% year="2016",
% volume="9",
% number="1",
% pages="1--29"
% }
%
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of activation functions (i.e., number of states in the GMM)
model.nbVar = 1; %Number of variables for the radial basis functions [s] (decay term)
model.nbVarPos = 2; %Number of motion variables [x1,x2]
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
model.polDeg = 4; %Degree of polynomial fit
nbData = 200; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
posId=[1:model.nbVarPos]; velId=[model.nbVarPos+1:2*model.nbVarPos]; accId=[2*model.nbVarPos+1:3*model.nbVarPos];
demos=[];
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
end
xTar = demos{1}.pos(:,end);
Data=[];
DataDMP=[];
for n=1:nbSamples
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
Data = [Data s(n).Data]; %Concatenation of the multiple demonstrations
%Nonlinear forcing term
DataDMP = [DataDMP, (s(n).Data(accId,:) - ...
(repmat(xTar,1,nbData)-s(n).Data(posId,:))*model.kP + s(n).Data(velId,:)*model.kV) ./ repmat(sIn,model.nbVarPos,1)];
end
%% Setting of the basis functions and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_timeBased(sIn, model);
%model = init_GMM_logBased(sIn, model); %Log-spread in s <-> equal spread in t
%model = init_GMM_kmeans(sIn, model);
%Set Sigma
for i=1:model.nbStates
model.Sigma(:,:,i) = 1E-2; %Setting of covariance
end
%Compute activation
H = zeros(model.nbStates,nbData);
for i=1:model.nbStates
H(i,:) = gaussPDF(sIn, model.Mu(:,i), model.Sigma(:,:,i));
end
H = H ./ repmat(sum(H,1),model.nbStates,1);
H2 = repmat(H,1,nbSamples);
%Nonlinear force profile retrieval (WLS version with polynomial)
X = [];
Xr = [];
for d=0:model.polDeg
X = [X, repmat(sIn.^d,1,nbSamples)'];
Xr = [Xr, sIn.^d'];
end
Y = DataDMP';
for i=1:model.nbStates
W = diag(H2(i,:));
MuF(:,:,i) = X'*W*X \ X'*W * Y; %Weighted least squares
end
Yr = zeros(nbData,model.nbVarPos);
for t=1:nbData
for i=1:model.nbStates
Yr(t,:) = Yr(t,:) + H(i,t) * Xr(t,:) * MuF(:,:,i);
end
end
%Motion retrieval with DMP
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
%Compute acceleration, velocity and position
ddx = L * [xTar-x; -dx] + Yr(t,:)' * sIn(t);
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('PaperPosition',[0 0 16 4],'position',[10,10,1300,500],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Spatial plot
axes('Position',[0 0 .2 1]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).Data(1,:),r(1).Data(2,:),'-','linewidth',3,'color',[.8 0 0]);
axis equal; axis square;
%Timeline plot of the nonlinear perturbing force
axes('Position',[.25 .58 .7 .4]); hold on;
for n=1:nbSamples
plot(sIn, DataDMP(1,(n-1)*nbData+1:n*nbData), '-','linewidth',2,'color',[.7 .7 .7]);
end
[~,id] = max(H,[],1);
for i=1:model.nbStates
Xr = [];
for d=0:model.polDeg
Xr = [Xr, sIn(id==i).^d'];
end
plot(sIn(id==i), Xr*MuF(:,1,i), '-','linewidth',6,'color',min(clrmap(i,:)+0.5,1));
end
plot(sIn, Yr(:,1), '-','linewidth',2,'color',[.8 0 0]);
axis([min(sIn) max(sIn) min(DataDMP(1,:)) max(DataDMP(1,:))]);
ylabel('$F_1$','fontsize',16,'interpreter','latex');
view(180,-90);
%Timeline plot of the basis functions activation
axes('Position',[.25 .12 .7 .4]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, H(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, H(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([min(sIn) max(sIn) 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP02.png');
%pause;
%close all;
......@@ -53,6 +53,7 @@ load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
%s(n).Data = interp1(1:size(demos{n}.pos,2), demos{n}.pos', linspace(1,size(demos{n}.pos,2),nbData));
Data = [Data s(n).Data];
end
......@@ -65,8 +66,8 @@ model = EM_GMM(Data, model);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,700,500]); hold on; box on;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
figure('position',[10,10,700,500]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.5 .5 .5]);
plotGMM(model.Mu, model.Sigma, [.8 0 0],.5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
......
......@@ -41,7 +41,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of states in the GMM
model.nbStates = 4; %Number of states in the GMM
model.nbVar = 3; %Number of variables [t,x1,x2]
model.dt = 0.001; %Time step duration
nbData = 200; %Length of each trajectory
......@@ -64,20 +64,20 @@ end
%model = init_GMM_kmeans(Data, model);
model = init_GMM_timeBased(Data, model);
model = EM_GMM(Data, model);
[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:3); %see Eq. (17)-(19)
[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:model.nbVar); %see Eq. (17)-(19)
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,500]);
%Plot GMM
subplot(1,2,1); hold on; box on; title('GMM');
plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
subplot(1,2,1); hold on; axis off; title('GMM');
plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.5 .5 .5]);
plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0], .5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
%Plot GMR
subplot(1,2,2); hold on; box on; title('GMR');
plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
subplot(1,2,2); hold on; axis off; title('GMR');
plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.5 .5 .5]);
plotGMM(DataOut, SigmaOut, [0 .8 0], .03);
plot(DataOut(1,:),DataOut(2,:),'-','linewidth',2,'color',[0 .4 0]);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
......
......@@ -56,6 +56,15 @@ disp('Load 3rd order tensor data...');
% multiplied by the number of demonstrations (M=5).
load('data/Data01.mat');
% %Regenerate data
% Data = zeros(model.nbVar, model.nbFrames, nbSamples*nbData);
% for n=1:nbSamples
% s(n).Data0 = s(n).Data0(2:end,:); %Remove time
% for m=1:model.nbFrames
% Data(:,m,(n-1)*nbData+1:n*nbData) = s(n).p(m).A \ (s(n).Data0 - repmat(s(n).p(m).b, 1, nbData));
% end
% end
%% TP-GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
......@@ -68,6 +77,23 @@ for n=1:nbSamples
[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
end
% %Products of linearly transformed Gaussians
% for n=1:nbSamples
% for i=1:model.nbStates
% SigmaTmp = zeros(model.nbVar);
% MuTmp = zeros(model.nbVar,1);
% for m=1:model.nbFrames
% MuP = s(n).p(m).A * model.Mu(:,m,i) + s(n).p(m).b;
% SigmaP = s(n).p(m).A * model.Sigma(:,:,m,i) * s(n).p(m).A';
% SigmaTmp = SigmaTmp + inv(SigmaP);
% MuTmp = MuTmp + SigmaP\MuP;
% end
% s(n).Sigma(:,:,i) = inv(SigmaTmp);
% s(n).Mu(:,i) = s(n).Sigma(:,:,i) * MuTmp;
% end
% end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
......
......@@ -41,7 +41,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM
model.nbStates = 6; %Number of Gaussians in the GMM
model.nbFrames = 2; %Number of candidate frames of reference
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
......@@ -51,14 +51,57 @@ model.dt = 0.01; %Time step duration
nbData = 200; %Number of datapoints in a trajectory
nbRepros = 5; %Number of reproductions
%Dynamical System settings (discrete version), see Eq. (33)
A = kron([1, model.dt; 0, 1], eye(model.nbVarPos));
B = kron([0; model.dt], eye(model.nbVarPos));
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
R = kron(eye(nbData-1),R);
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %Integration with Euler method
% 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));
%Integration with higher order Taylor series expansion
A1d = zeros(model.nbDeriv);
for i=0:model.nbDeriv-1
A1d = A1d + diag(ones(model.nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
end
B1d = zeros(model.nbDeriv,1);
for i=1:model.nbDeriv
B1d(model.nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
end
A = kron(A1d, eye(model.nbVarPos)); %Discrete nD
B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
% %Conversion with control toolbox
% Ac1d = diag(ones(model.nbDeriv-1,1),1); %Continuous 1D
% Bc1d = [zeros(model.nbDeriv-1,1); 1]; %Continuous 1D
% Cc1d = [1, zeros(1,model.nbDeriv-1)]; %Continuous 1D
% sysd = c2d(ss(Ac1d,Bc1d,Cc1d,0), model.dt);
% A = kron(sysd.a, eye(model.nbVarPos)); %Discrete nD
% B = kron(sysd.b, eye(model.nbVarPos)); %Discrete nD
%Construct Su and Sx matrices, see Eq. (35)
Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
Sx = kron(ones(nbData,1),eye(model.nbVar));
M = B;
for n=2:nbData
%Build Sx matrix
id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
Sx(id1,:) = Sx(id1,:) * A;
%Build Su matrix
id1 = (n-1)*model.nbVar+1:n*model.nbVar;
id2 = 1:(n-1)*model.nbVarPos;
Su(id1,id2) = M;
M = [A*M(:,1:model.nbVarPos), M];
end
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...');
......@@ -70,72 +113,34 @@ disp('Load 3rd order tensor data...');
% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5).
load('data/DataWithDeriv01.mat');
% %Convert position data to position+velocity data
% load('data/Data01.mat');
% %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
% model.nbVar = model.nbVarPos * model.nbDeriv;
% Data = zeros(model.nbVar, model.nbFrames, nbSamples*nbData);
% for n=1:nbSamples
% s(n).Data = zeros(model.nbVar,model.nbFrames,nbData);
% s(n).Data0 = s(n).Data0(2:end,:); %Remove time
% DataTmp = s(n).Data0;
% for k=1:model.nbDeriv-1
% DataTmp = [DataTmp; s(n).Data0*D^k]; %Compute derivatives
% end
% for m=1:model.nbFrames
% s(n).p(m).b = [s(n).p(m).b; zeros((model.nbDeriv-1)*model.nbVarPos,1)];
% s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A);
% s(n).Data(:,m,:) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbData));
% Data(:,m,(n-1)*nbData+1:n*nbData) = s(n).Data(:,m,:);
% end
% end
% %Save new dataset including derivatives
% save('data/DataWithDeriv01.mat', 'Data','s','nbSamples');
%% TP-GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of TP-GMM with EM...');
%model = init_tensorGMM_kmeans(Data, model); %Initialization
model = init_tensorGMM_kbins(s, model);
%Initialization based on position data
model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model);
[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0);
model.Priors = model0.Priors;
for i=1:model.nbStates
for m=1:model.nbFrames
DataTmp = squeeze(Data(:,m,:));
model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)';
DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples);
model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
end
end
% %Initialization based on position data
% model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model);
% [~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0);
% model.Priors = model0.Priors;
% for i=1:model.nbStates
% for m=1:model.nbFrames
% DataTmp = squeeze(Data(:,m,:));
% model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)';
% DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples);
% model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
% end
% end
% [~, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
% model.Pix = GAMMA ./ repmat(sum(GAMMA,2),1,nbData*nbSamples);
model = EM_tensorGMM(Data, model);
%% Reproduction with LQR for the task parameters used to train the model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with batch LQR...');
%Construct Su and Sx matrices, see Eq. (35)
Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
Sx = kron(ones(nbData,1),eye(model.nbVar));
M = B;
for n=2:nbData
%Build Sx matrix
id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
Sx(id1,:) = Sx(id1,:) * A;
%Build Su matrix
id1 = (n-1)*model.nbVar+1:n*model.nbVar;
id2 = 1:(n-1)*model.nbVarPos;
Su(id1,id2) = M;
M = [A*M(:,1:model.nbVarPos), M];
end
%Reproductions
for n=1:nbSamples
%Reconstruct GMM
[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
......@@ -143,8 +148,7 @@ for n=1:nbSamples
[~,s(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1
%Build stepwise reference trajectory, see Eq. (27)
MuQ = reshape(s(n).Mu(:,s(n).q), model.nbVar*nbData, 1);
SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(s(n).Sigma(:,:,s(n).q), model.nbVar, model.nbVar*nbData)) ...
.* kron(eye(nbData), ones(model.nbVar));
SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(s(n).Sigma(:,:,s(n).q), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
%Batch LQR (unconstrained linear MPC), see Eq. (37)
SuInvSigmaQ = Su' / SigmaQ;
Rq = SuInvSigmaQ * Su + R;
......@@ -172,8 +176,7 @@ for n=1:nbRepros
[~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1
%Build stepwise reference trajectory, see Eq. (27)
MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVar*nbData, 1);
SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(rnew(n).Sigma(:,:,rnew(n).q), model.nbVar, model.nbVar*nbData)) ...
.* kron(eye(nbData), ones(model.nbVar));
SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(rnew(n).Sigma(:,:,rnew(n).q), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
%Batch LQR (unconstrained linear MPC), see Eq. (37)
SuInvSigmaQ = Su' / SigmaQ;
Rq = SuInvSigmaQ * Su + R;
......@@ -223,7 +226,7 @@ for n=1:nbSamples
end
for n=1:nbSamples
%Plot Gaussians
plotGMM(s(n).Mu(1:2,:,1), s(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4);
plotGMM(s(n).Mu(1:2,:), s(n).Sigma(1:2,1:2,:), [.5 .5 .5], .4);
end