Skip to content

GitLab

Commit cada04c3 authored by Sylvain Calinon's avatar Sylvain Calinon
Browse files

New HMM/HSMM and related examples added

parent 2c3d6d31
Hide whitespace changes
Inline Side-by-side
README.md
View file @ cada04c3
......@@ -75,7 +75,7 @@ Examples starting with `demo_` can be run from Matlab/GNU Octave.
Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
Written by Sylvain Calinon, http://calinon.ch/
Maintained by Sylvain Calinon, http://calinon.ch/
PbDlib is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License version 3 as
......@@ -119,13 +119,30 @@ All the examples are located in the main folder, and the functions are located i
| 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 |
| demo_GMM02 | GMM with different covariance structures |
| demo_GMR01 | GMM and time-based Gaussian mixture regression (GMR) used for reproduction |
| demo_GPR01 | Use of Gaussian process regression (GPR) as a task-parameterized model, with DS-GMR used to retrieve continuous movements |
| demo_GMR02 | GMR computed with precision matrices instead of covariances |
| demo_GMR03 | Chain rule with Gaussian conditioning |
| demo_GMR_3Dviz01 | 3D visualization of a GMM with time-based GMR used for reproduction |
| demo_GMR_polyFit01 | Polynomial fitting with multivariate GMR |
| demo_GPR01 | Gaussian process regression (GPR) |
| demo_GPR02 | GPR with stochastic samples from the prior and the posterior |
| demo_GPR03 | GPR with periodic kernel function |
| demo_GPR_TP01 | Use of GPR as a task-parameterized model, with DS-GMR used to retrieve continuous movements |
| demo_grabData01 | Collect movement data from mouse cursor |
| demo_HDDC01 | High Dimensional Data Clustering (HDDC, or HD-GMM) |
| demo_HMM01 | Hidden Markov model (HMM) with single Gaussian as emission distribution |
| demo_HMM_Viterbi01 | Viterbi decoding in HMM to estimate best state sequence from observations |
| demo_HSMM01 | Variable duration model implemented as a hidden semi-Markov model (HSMM), by encoding the state duration after EM |
| demo_iterativeLQR01 | Controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, unconstrained linear MPC), by relying on a GMM encoding of position and velocity data (see also demo_batchLQR01) |
| demo_iterativeLQR02 | Same as demo_iterativeLQR01 with only position data |
| demo_MFA01 | Mixture of factor analyzers (MFA) |
| demo_MPPCA01 | Mixture of probabilistic principal component analyzers (MPPCA) |
| demo_regularization01 | Regularization of GMM parameters with minimum admissible eigenvalue |
| demo_regularization02 | Regularization of GMM parameters with the addition of a small circular covariance |
| demo_SEDS01 | Continuous autonomous dynamical system with state-space encoding using GMM, with GMR used for reproduction by using a constrained optimization similar to the SEDS approach |
| demo_SEDS_discrete01 | Discrete autonomous dynamical system with state-space encoding using GMM, with GMR used for reproduction by using a constrained optimization similar to the SEDS approach |
| demo_semitiedGMM01 | Semi-tied Gaussian Mixture Model by tying the covariance matrices of a GMM with a set of common basis vectors |
| demo_stdPGMM01 | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements |
| demo_testDampingRatio01 | Test with critically damped system and ideal underdamped system |
| demo_testLQR01 | Test of linear quadratic regulation (LQR) with different variance in the data |
......@@ -145,8 +162,12 @@ All the examples are located in the main folder, and the functions are located i
| 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_TPproMP01 | Task-parameterized probabilistic movement primitives (TP-ProMP) |
| demo_TPtrajDistrib01 | Task-parameterized model with trajectory distribution and eigendecomposition |
| demo_TPtrajGMM01 | Task-parameterized model with trajectory-GMM encoding |
| demo_trajDistrib01 | Stochastic sampling with Gaussian trajectory distribution |
| demo_trajGMM01 | Reproduction of trajectory with a GMM with dynamic features (trajectory-GMM) |
| demo_trajHSMM01 | Trajectory synthesis with an HSMM with dynamic features (trajectory-HSMM) |
| demo_trajMFA01 | Trajectory model with either a mixture of factor analysers (MFA), a mixture of probabilistic principal component analyzers (MPPCA), or a high-dimensional data clustering approach (HD-GMM) |
| demoIK_nullspace_TPGMM01 | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) |
| demoIK_pointing_TPGMM01 | Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), by using a single Euler orientation angle and 3 DOFs robot) |
demo_DPMeans_Online01.m
View file @ cada04c3
function demo_DPMeans_Online01
%Online clustering with DP-Means algorithm
%Danilo Bruno, 2015
%
% 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{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",
% }
%
% Written by Danilo Bruno and Sylvain Calinon, 2015
%
% 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/');
......
demo_GMM02.m 0 → 100644
View file @ cada04c3
function demo_GMM02
% GMM with different covariance structures.
%
% 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 states in the GMM
model.nbVar = 2; %Number of variables [x1,x2]
nbData = 100; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%Parameters of the EM algorithm
nbMinSteps = 50; %Minimum number of iterations allowed
nbMaxSteps = 200; %Maximum number of iterations allowed
maxDiffLL = 1E-5; %Likelihood increase threshold to stop the algorithm
diagRegularizationFactor = 1E-2; %Regularization term is optional
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/2Dletters/C.mat');
Data=[];
a=pi/3;
R = [cos(a) sin(a); -sin(a) cos(a)];
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
Data = [Data, R*s(n).Data];
end
%Initialization
%model = init_GMM_kmeans(Data, model);
model = init_GMM_timeBased([repmat(1:nbData,1,nbSamples); Data], model);
model.Mu = model.Mu(2:end,:);
model.Sigma = model.Sigma(2:end,2:end,:);
nbData = nbData * nbSamples;
%% EM with isotropic covariances
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for nbIter=1:nbMaxSteps
fprintf('.');
%E-step
[L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
%M-step
for i=1:model.nbStates
%Update Priors
model.Priors(i) = sum(GAMMA(i,:)) / nbData;
%Update Mu
model.Mu(:,i) = Data * GAMMA2(i,:)';
%Update Sigma
DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
model.Sigma(:,:,i) = diag(diag(DataTmp * diag(GAMMA2(i,:)) * DataTmp')) + eye(size(Data,1)) * diagRegularizationFactor;
end
model.Sigma = repmat(eye(model.nbVar),[1 1 model.nbStates]) * mean(mean(mean(model.Sigma)));
%Compute average log-likelihood
LL(nbIter) = sum(log(sum(L,1))) / nbData;
%Stop the algorithm if EM converged (small change of LL)
if nbIter>nbMinSteps
if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
disp(['EM converged after ' num2str(nbIter) ' iterations.']);
break;
end
end
end
disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
m1 = model;
%% EM with diagonal covariances
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for nbIter=1:nbMaxSteps
fprintf('.');
%E-step
[L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
%M-step
for i=1:model.nbStates
%Update Priors
model.Priors(i) = sum(GAMMA(i,:)) / nbData;
%Update Mu
model.Mu(:,i) = Data * GAMMA2(i,:)';
%Update Sigma
DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
model.Sigma(:,:,i) = diag(diag(DataTmp * diag(GAMMA2(i,:)) * DataTmp')) + eye(size(Data,1)) * diagRegularizationFactor;
end
%Compute average log-likelihood
LL(nbIter) = sum(log(sum(L,1))) / nbData;
%Stop the algorithm if EM converged (small change of LL)
if nbIter>nbMinSteps
if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
disp(['EM converged after ' num2str(nbIter) ' iterations.']);
break;
end
end
end
disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
m2 = model;
%% EM for full covariance matrices
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[m3,~,LL] = EM_GMM(Data, model);
%% EM with tied covariances
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for nbIter=1:nbMaxSteps
fprintf('.');
%E-step
[L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below
GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
%M-step
for i=1:model.nbStates
%Update Priors
model.Priors(i) = sum(GAMMA(i,:)) / nbData;
%Update Mu
model.Mu(:,i) = Data * GAMMA2(i,:)';
%Update Sigma
DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(size(Data,1)) * diagRegularizationFactor;
end
model.Sigma = repmat(mean(model.Sigma,3), [1 1 model.nbStates]);
%Compute average log-likelihood
LL(nbIter) = sum(log(sum(L,1))) / nbData;
%Stop the algorithm if EM converged (small change of LL)
if nbIter>nbMinSteps
if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
disp(['EM converged after ' num2str(nbIter) ' iterations.']);
break;
end
end
end
disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
m4 = model;
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('PaperPosition',[0 0 16 5],'position',[10,10,1300,400]);
%Isotropic covariance
subplot(1,4,1); hold on; axis off; title('Isotropic','fontsize',14);
plotGMM(m1.Mu, m1.Sigma, [.8 0 0], .5);
plot(Data(1,:),Data(2,:),'.','markersize',12,'color',[.5 .5 .5]);
axis([min(Data(1,:))-1E0 max(Data(1,:))+1E0 min(Data(2,:))-1E0 max(Data(2,:))+1E0]); axis equal;
%Diagonal covariance
subplot(1,4,2); hold on; axis off; title('Diagonal','fontsize',14);
plotGMM(m2.Mu, m2.Sigma, [.8 0 0], .5);
plot(Data(1,:),Data(2,:),'.','markersize',12,'color',[.5 .5 .5]);
axis([min(Data(1,:))-1E0 max(Data(1,:))+1E0 min(Data(2,:))-1E0 max(Data(2,:))+1E0]); axis equal;
%Full covariance
subplot(1,4,3); hold on; axis off; title('Full','fontsize',14);
plotGMM(m3.Mu, m3.Sigma, [.8 0 0], .5);
plot(Data(1,:),Data(2,:),'.','markersize',12,'color',[.5 .5 .5]);
axis([min(Data(1,:))-1E0 max(Data(1,:))+1E0 min(Data(2,:))-1E0 max(Data(2,:))+1E0]); axis equal;
%Tied covariance
subplot(1,4,4); hold on; axis off; title('Tied','fontsize',14);
plotGMM(m4.Mu, m4.Sigma, [.8 0 0], .5);
plot(Data(1,:),Data(2,:),'.','markersize',12,'color',[.5 .5 .5]);
axis([min(Data(1,:))-1E0 max(Data(1,:))+1E0 min(Data(2,:))-1E0 max(Data(2,:))+1E0]); axis equal;
%print('-dpng','graphs/demo_GMM02.png');
%pause;
%close all;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [L, GAMMA] = computeGamma(Data, model)
L = zeros(model.nbStates,size(Data,2));
for i=1:model.nbStates
L(i,:) = model.Priors(i) * gaussPDF(Data, model.Mu(:,i), model.Sigma(:,:,i));
end
GAMMA = L ./ repmat(sum(L,1)+realmin, model.nbStates, 1);
end
demo_GMR02.m 0 → 100644
View file @ cada04c3
function demo_GMR02
% GMR computed with precision matrices instead of covariances
%
% 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 = 3; %Number of states in the GMM
model.nbVar = 3; %Number of variables [t,x1,x2]
nbData = 20; %Length of each trajectory
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/2Dletters/G.mat');
nbSamples = length(demos);
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
Data = [Data [1:nbData; s(n).Data]];
end
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_timeBased(Data, model);
model = EM_GMM(Data, model);
in=1; out=2:3;
DataIn = Data(in,:);
t=1;
for i=1:model.nbStates
model.P(:,:,i) = inv(model.Sigma(:,:,i));
%Regression based on covariance matrices
MuOut = model.Mu(out,i) + model.Sigma(out,in,i)/model.Sigma(in,in,i) * (DataIn(:,t)-model.Mu(in,i))
SigmaOut = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i)
%Regression based on precision matrices
MuOut = model.Mu(out,i) - model.P(out,out,i)\model.P(out,in,i) * (DataIn(:,t)-model.Mu(in,i))
SigmaOut = inv(model.P(out,out,i))
end
demo_GMR03.m 0 → 100644
View file @ cada04c3
function demo_GMR03
% Chain rule with Gaussian conditioning
%
% 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.nbVar = 3; %Number of variables [t,x1,x2]
nbData = 20; %Length of each trajectory
%% Generate data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Data = rand(model.nbVar, nbData);
model.Mu = mean(Data,2);
model.Sigma = cov(Data');
%% Chain rule
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1;
gaussPDF(Data(:,t), model.Mu, model.Sigma)
p=1;
for i=1:model.nbVar
in = 1:i-1;
out = i;
%Gaussian conditioning
MuOut = model.Mu(out) + model.Sigma(out,in)/model.Sigma(in,in) * (Data(in,t) - model.Mu(in));
SigmaOut = model.Sigma(out,out) - model.Sigma(out,in)/model.Sigma(in,in) * model.Sigma(in,out);
p = p * gaussPDF(Data(out,t), MuOut, SigmaOut);
end
p
demo_GMR_3Dviz01.m 0 → 100644
View file @ cada04c3
function demo_GMR_3Dviz01
% 3D visualization of a Gaussian mixture model (GMM) with time-based Gaussian mixture
% regression (GMR) used for reproduction
%
% 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 states in the GMM
model.nbVar = 5; %Number of variables [t,x1,x2,x3,x4]
model.dt = 0.01; %Time step duration
nbData = 100; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/2Dletters/G.mat'); %Load x1,x2 variables
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
end
demos=[];
load('data/2Dletters/C.mat'); %Load x3,x4 variables
Data=[];
for n=1:nbSamples
s(n).Data = [[1:nbData]*model.dt; s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
Data = [Data s(n).Data];
end
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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:model.nbVar); %see Eq. (17)-(19)
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Display figure (can take some time)...');
figure('position',[10,10,1000,500]);
%Plot GMM
subplot(1,2,1); hold on; box on; title('GMM');
plotGMM3D(model.Mu(2:4,:), model.Sigma(2:4,2:4,:), [.8 0 0], .3);
%plot3(Data(2,:),Data(3,:),Data(4,:),'.','markersize',8,'color',[.7 .7 .7]);
for n=1:nbSamples
dTmp = [Data(2:4,(n-1)*nbData+1:n*nbData) fliplr(Data(2:4,(n-1)*nbData+1:n*nbData))];
patch(dTmp(1,:),dTmp(2,:),dTmp(3,:), [.5,.5,.5],'facealpha',0,'linewidth',2,'edgecolor',[.5,.5,.5],'edgealpha',.5);
end
view(3); axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]); set(gca,'Ztick',[]);
xlabel('x_1'); ylabel('x_2'); zlabel('x_3');
%Plot GMR
subplot(1,2,2); hold on; box on; title('GMR');
plotGMM3D(DataOut(1:3,1:2:end), SigmaOut(1:3,1:3,1:2:end), [0 .8 0], .2, 2);
for n=1:nbSamples
dTmp = [Data(2:4,(n-1)*nbData+1:n*nbData) fliplr(Data(2:4,(n-1)*nbData+1:n*nbData))];
patch(dTmp(1,:),dTmp(2,:),dTmp(3,:), [.5,.5,.5],'facealpha',0,'linewidth',2,'edgecolor',[.5,.5,.5],'edgealpha',.5);
end
plot3(DataOut(1,:),DataOut(2,:),DataOut(3,:),'-','linewidth',4,'color',[0 .4 0]);
view(3); axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]); set(gca,'Ztick',[]);
xlabel('x_1'); ylabel('x_2'); zlabel('x_3');
%print('-dpng','graphs/demo_GMR_3Dviz01.png');
%pause;
%close all;
demo_GPR01.m
View file @ cada04c3
function demo_GPR01
% Gaussian processs regression (GPR) used for task adaptation, with DS-GMR used to
% retrieve continuous movements.
%
% Gaussian process regression (GPR)
%
% 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,
......@@ -41,224 +40,80 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM
model.nbFrames = 2; %Number of candidate frames of reference
model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain (for DS-GMR)
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 8; %Number of reproductions with new situations randomly generated
nbVar = 3; %Dimension of datapoint (t,x1,x2)
nbData = 20; %Number of datapoints
nbDataRepro = 100; %Number of datapoints for reproduction
nbSamples = 1; %Number of demonstrations
p(1)=1E0; p(2)=1E1; p(3)=1E-2; %GPR parameters
%% Load 3rd order tensor data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load motion 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=3 the dimension of a
% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
% multiplied by the number of demonstrations (nbSamples=5).
load('data/DataLQR01.mat');
%% Transformation of 'Data' to learn the path of the spring-damper system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbD = s(1).nbData;
nbVarOut = model.nbVar - 1;
%Create transformation matrix to compute [X; DX; DDX]
D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
D(end,end) = 0;
%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(nbVarOut));
%Create data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
Data = s(1).Data0(1,:);
demos=[];