Commit 4082df78 authored by Sylvain Calinon's avatar Sylvain Calinon
Browse files

Initial commit

parent 703a5e8a
# pbdlib-matlab # pbdlib-matlab
### Compatibility PbDLib is a set of tools combining statistical learning, dynamical systems and optimal control approaches for programming-by-demonstration applications.
The codes have been tested with both Matlab and GNU Octave.
### Usage
Examples starting with 'demo_' can be run from Matlab/GNU Octave.
### References
Calinon, S. (in preparation). A tutorial on task-parameterized movement learning and retrieval. The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, http://calinon.ch.
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention More information can be found on: http://programming-by-demonstration.org/lib/
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description ### Compatibility
Demonstration of a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper systems
acting in multiple frames of reference. Each candidate coordinate system observes a set of demonstrations from its own
perspective, by extracting an attractor path whose variations depend on the relevance of the frame through the task.
This information is exploited to generate a new attractor path corresponding to new situations (new positions and
orientation of the frames), while the predicted covariances are exploited by a linear quadratic regulator (LQR) to
estimate the stiffness and damping feedback terms of the spring-damper systems, resulting in a minimal intervention
control strategy.
### Datasets
The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within The codes are compatible with both Matlab or GNU Octave.
the context of the AMARSI European Project. Reference: S.M. Khansari-Zadeh and A. Billard, "Learning Stable Non-Linear
Dynamical Systems with Gaussian Mixture Models", IEEE Transaction on Robotics, 2011.
### Contact ### Usage
Sylvain Calinon Examples starting with `demo_` can be run from Matlab/GNU Octave.
http://programming-by-demonstration.org/lib/
This source code is given for free! In exchange, we would be grateful if you cite the following reference in any ### References
academic publication that uses this code or part of it:
@inproceedings{Calinon14ICRA, Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used some parts of these codes.
```
@article{Calinon15,
author="Calinon, S.",
title="A tutorial on task-parameterized movement learning and retrieval",
year="2015",
}
```
```
@inproceedings{Calinon14ICRA,
author="Calinon, S. and Bruno, D. and Caldwell, D. G.", author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
title="A task-parameterized probabilistic model with minimal intervention control", title="A task-parameterized probabilistic model with minimal intervention control",
booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})", booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
...@@ -47,5 +34,53 @@ ...@@ -47,5 +34,53 @@
month="May-June", month="May-June",
address="Hong Kong, China", address="Hong Kong, China",
pages="3339--3344" pages="3339--3344"
} }
```
### Dataset
The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within the context of the AMARSI European Project. Reference: S.M. Khansari-Zadeh and A. Billard, "Learning Stable Non-Linear Dynamical Systems with Gaussian Mixture Models", IEEE Transaction on Robotics, 2011.
### List of examples
All the examples are located in the main folder, and the functions are located in the `m_fcts` folder.
| Filename | Description |
|----------|-------------|
| benchmark_DS_GP_GMM01 | Benchmark of task-parameterized model based on Gaussian process regression, with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction |
| benchmark_DS_GP_raw01 | Benchmark of task-parameterized model based on Gaussian process regression, with raw trajectory, and spring-damper system used for reproduction |
| benchmark_DS_PGMM01 | Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction |
| benchmark_DS_TP_GMM01 | Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction |
| benchmark_DS_TP_GP01 | Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) |
| 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_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_GMM01 | Gaussian mixture model (GMM) parameters estimation |
| demo_GMR01 | Gaussian mixture model (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_HDDC01 | High Dimensional Data Clustering (HDDC, or HD-GMM) model from Bouveyron (2007) |
| demo_iterativeLQR01 | Controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_batchLQR01) |
| demo_MFA01 | Mixture of factor analyzers (MFA) |
| demo_MPPCA01 | Mixture of probabilistic principal component analyzers (MPPCA) |
| demo_stdPGMM01 | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements |
| demo_testLQR01 | Test of the linear quadratic regulation with different variance in the data |
| demo_testLQR02 | Test of the linear quadratic regulation (LQR) with evaluation of the damping ratio found by the system |
| demo_testLQR03 | Comparison of linear quadratic regulators (LQR) with finite and infinite time horizons |
| demo_TPbatchLQR01 | Task-parameterized GMM encoding position and velocity data, combined with a batch solution of linear quadratic optimal control (unconstrained linear MPC) |
| demo_TPbatchLQR02 | Batch solution of a linear quadratic optimal control (unconstrained linear MPC) acting in multiple frames, which is equivalent to TP-GMM combined with LQR |
| demo_TPGMM01 | Task-parameterized Gaussian mixture model (TP-GMM) encoding |
| demo_TPGMR01 | Task-parameterized Gaussian mixture model (TP-GMM), with GMR used for reproduction (without dynamical system) |
| demo_TPGMR_DS01 | Dynamical system with constant gains used with a task-parameterized model |
| demo_TPGMR_LQR01 | Finite horizon LQR used with a task-parameterized model
| demo_TPGMR_LQR02 | Infinite horizon LQR used with a task-parameterized model
| 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_TPMPPCA01 | Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) |
| demo_TPtrajGMM01 | Task-parameterized model with trajectory-GMM encoding |
| demo_trajGMM01 | Reproduction of trajectory with a GMM with dynamic features (trajectory 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) |
function benchmark_DS_GP_GMM01 function benchmark_DS_GP_GMM01
%Benchmark of task-parameterized model based on Gaussian process regression, % Benchmark of task-parameterized model based on Gaussian process regression,
%with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction % with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction
%Sylvain Calinon, 2015 %
% 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/'); addpath('./m_fcts/');
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM model.nbStates = 3; %Number of Gaussians in the GMM
...@@ -14,8 +26,8 @@ model.dt = 0.01; %Time step ...@@ -14,8 +26,8 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated nbRepros = 4; %Number of reproductions with new situations randomly generated
nbVarOut = model.nbVar-1; nbVarOut = model.nbVar-1; %(here: x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
%% Load 3rd order tensor data %% Load 3rd order tensor data
...@@ -25,8 +37,8 @@ disp('Load 3rd order tensor data...'); ...@@ -25,8 +37,8 @@ disp('Load 3rd order tensor data...');
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and % 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 % 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 % 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) % 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 (5). % multiplied by the number of demonstrations (M=5).
load('data/DataLQR01.mat'); load('data/DataLQR01.mat');
...@@ -40,7 +52,7 @@ D(end,end) = 0; ...@@ -40,7 +52,7 @@ D(end,end) = 0;
%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP %Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
K1d = [1, model.kV/model.kP, 1/model.kP]; K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(nbVarOut)); K = kron(K1d,eye(nbVarOut));
%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf %Compute data with derivatives
%Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); %Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
Data = s(1).Data0(1,:); Data = s(1).Data0(1,:);
for n=1:nbSamples for n=1:nbSamples
...@@ -102,14 +114,14 @@ for n=1:nbSamples ...@@ -102,14 +114,14 @@ for n=1:nbSamples
end end
% %Retrieval of attractor path through GMR % %Retrieval of attractor path through GMR
% currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf % currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
% %
% %Motion retrieval with spring-damper system % %Motion retrieval with spring-damper system
% x = s(n).p(1).b(2:model.nbVar); % x = s(n).p(1).b(2:model.nbVar);
% dx = zeros(nbVarOut,1); % dx = zeros(nbVarOut,1);
% for t=1:s(n).nbData % for t=1:s(n).nbData
% %Compute acceleration, velocity and position % %Compute acceleration, velocity and position
% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf % ddx = -L * [x-currTar(:,t); dx];
% dx = dx + ddx * model.dt; % dx = dx + ddx * model.dt;
% x = x + dx * model.dt; % x = x + dx * model.dt;
% r(n).Data(:,t) = x; % r(n).Data(:,t) = x;
...@@ -146,14 +158,14 @@ for n=1:nbRepros ...@@ -146,14 +158,14 @@ for n=1:nbRepros
end end
%Retrieval of attractor path through GMR %Retrieval of attractor path through GMR
[rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf [rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
%Motion retrieval with spring-damper system %Motion retrieval with spring-damper system
x = rnew(n).p(1).b(2:model.nbVar); x = rnew(n).p(1).b(2:model.nbVar);
dx = zeros(nbVarOut,1); dx = zeros(nbVarOut,1);
for t=1:nbD for t=1:nbD
%Compute acceleration, velocity and position %Compute acceleration, velocity and position
ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf ddx = -L * [x-rnew(n).currTar(:,t); dx];
dx = dx + ddx * model.dt; dx = dx + ddx * model.dt;
x = x + dx * model.dt; x = x + dx * model.dt;
rnew(n).Data(:,t) = x; rnew(n).Data(:,t) = x;
...@@ -180,9 +192,10 @@ for n=1:nbSamples ...@@ -180,9 +192,10 @@ for n=1:nbSamples
plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04); plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
end end
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png'); %print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png');
%Plot reproductions in new situations %Plot reproductions in new situations
disp('[Press enter to see next reproduction attempt]');
h=[]; h=[];
for n=1:nbRepros for n=1:nbRepros
delete(h); delete(h);
...@@ -193,8 +206,8 @@ for n=1:nbRepros ...@@ -193,8 +206,8 @@ for n=1:nbRepros
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']); %print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
%pause pause
end end
pause; pause;
......
function benchmark_DS_GP_raw01 function benchmark_DS_GP_raw01
%Benchmark of task-parameterized model based on Gaussian process regression, % Benchmark of task-parameterized model based on Gaussian process regression,
%with raw trajectory, and spring-damper system used for reproduction % with raw trajectory, and spring-damper system used for reproduction
%Sylvain Calinon, 2015 %
% 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/'); addpath('./m_fcts/');
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbFrames = 2; %Number of candidate frames of reference model.nbFrames = 2; %Number of candidate frames of reference
...@@ -15,6 +27,7 @@ model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) ...@@ -15,6 +27,7 @@ model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated nbRepros = 4; %Number of reproductions with new situations randomly generated
L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV]; L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV];
%% Load 3rd order tensor data %% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...'); disp('Load 3rd order tensor data...');
...@@ -22,8 +35,8 @@ disp('Load 3rd order tensor data...'); ...@@ -22,8 +35,8 @@ disp('Load 3rd order tensor data...');
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and % 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 % 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 % 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) % 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 (5). % multiplied by the number of demonstrations (M=5).
load('data/DataLQR01.mat'); load('data/DataLQR01.mat');
...@@ -36,7 +49,7 @@ D(end,end) = 0; ...@@ -36,7 +49,7 @@ D(end,end) = 0;
%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP %Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
K1d = [1, model.kV/model.kP, 1/model.kP]; K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVar)); K = kron(K1d,eye(model.nbVar));
%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf %Compute derivatives
%Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); %Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
Data = s(1).Data0(1,:); Data = s(1).Data0(1,:);
for n=1:nbSamples for n=1:nbSamples
...@@ -45,6 +58,7 @@ for n=1:nbSamples ...@@ -45,6 +58,7 @@ for n=1:nbSamples
Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples) Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
end end
%% GPR with raw trajectory encoding %% GPR with raw trajectory encoding
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of GPR with raw trajectory encoding:'); fprintf('Parameters estimation of GPR with raw trajectory encoding:');
...@@ -70,7 +84,7 @@ end ...@@ -70,7 +84,7 @@ end
% dx = zeros(model.nbVar,1); % dx = zeros(model.nbVar,1);
% for t=1:s(n).nbData % for t=1:s(n).nbData
% %Compute acceleration, velocity and position % %Compute acceleration, velocity and position
% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf % ddx = -L * [x-currTar(:,t); dx];
% dx = dx + ddx * model.dt; % dx = dx + ddx * model.dt;
% x = x + dx * model.dt; % x = x + dx * model.dt;
% r(n).Data(:,t) = x; % r(n).Data(:,t) = x;
...@@ -101,7 +115,7 @@ for n=1:nbRepros ...@@ -101,7 +115,7 @@ for n=1:nbRepros
dx = zeros(model.nbVar,1); dx = zeros(model.nbVar,1);
for t=1:nbD for t=1:nbD
%Compute acceleration, velocity and position %Compute acceleration, velocity and position
ddx = -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf ddx = -L * [x-rnew(n).currTar(:,t); dx];
dx = dx + ddx * model.dt; dx = dx + ddx * model.dt;
x = x + dx * model.dt; x = x + dx * model.dt;
rnew(n).Data(:,t) = x; rnew(n).Data(:,t) = x;
...@@ -125,9 +139,10 @@ for n=1:nbSamples ...@@ -125,9 +139,10 @@ for n=1:nbSamples
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04); [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
end end
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png'); %print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png');
%Plot reproductions in new situations %Plot reproductions in new situations
disp('[Press enter to see next reproduction attempt]');
h=[]; h=[];
for n=1:nbRepros for n=1:nbRepros
delete(h); delete(h);
...@@ -137,8 +152,8 @@ for n=1:nbRepros ...@@ -137,8 +152,8 @@ for n=1:nbRepros
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']); %print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']);
%pause pause;
end end
pause; pause;
......
function benchmark_DS_PGMM01 function benchmark_DS_PGMM01
%Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction % Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction
%Sylvain Calinon, 2015 %
% 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/'); addpath('./m_fcts/');
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of Gaussians in the GMM model.nbStates = 3; %Number of Gaussians in the GMM
...@@ -13,8 +25,8 @@ model.dt = 0.01; %Time step ...@@ -13,8 +25,8 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio) model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated nbRepros = 4; %Number of reproductions with new situations randomly generated
nbVarOut = model.nbVar - 1; nbVarOut = model.nbVar-1; %(here, x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
%% Load 3rd order tensor data %% Load 3rd order tensor data
...@@ -24,8 +36,8 @@ disp('Load 3rd order tensor data...'); ...@@ -24,8 +36,8 @@ disp('Load 3rd order tensor data...');
% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and % 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 % 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 % 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) % 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 (5). % multiplied by the number of demonstrations (M=5).
load('data/DataLQR01.mat'); load('data/DataLQR01.mat');
...@@ -38,7 +50,7 @@ D(end,end) = 0; ...@@ -38,7 +50,7 @@ D(end,end) = 0;
%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP %Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
K1d = [1, model.kV/model.kP, 1/model.kP]; K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(nbVarOut)); K = kron(K1d,eye(nbVarOut));
%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf %Compute derivatives
Data = []; Data = [];
for n=1:nbSamples for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:); DataTmp = s(n).Data0(2:end,:);
...@@ -51,9 +63,10 @@ end ...@@ -51,9 +63,10 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of PGMM with EM:'); fprintf('Parameters estimation of PGMM with EM:');
for n=1:nbSamples for n=1:nbSamples
% %Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf % %Task parameters rearranged as a vector (position and orientation), see Eq. (48)
% s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1]; % s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1];
%Task parameters rearranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
%Task parameters rearranged as a vector (position only), see Eq. (48)
s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1]; s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1];
end end
...@@ -61,10 +74,12 @@ end ...@@ -61,10 +74,12 @@ end
% %model = init_GMM_kmeans(Data, model); % %model = init_GMM_kmeans(Data, model);
% model = init_GMM_timeBased(Data, model); % model = init_GMM_timeBased(Data, model);
% model = EM_GMM(Data, model); % model = EM_GMM(Data, model);
for i=1:model.nbStates for i=1:model.nbStates
% %Initialization of parameters based on standard GMM % %Initialization of parameters based on standard GMM
% model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1)); % model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1));
% model.ZMu(:,end,i) = model.Mu(:,i); % model.ZMu(:,end,i) = model.Mu(:,i);
%Random initialization of parameters %Random initialization of parameters
model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1)); model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1));
model.Sigma(:,:,i) = eye(model.nbVar); model.Sigma(:,:,i) = eye(model.nbVar);
...@@ -83,17 +98,18 @@ nbVarOut = model.nbVar-1; ...@@ -83,17 +98,18 @@ nbVarOut = model.nbVar-1;
for n=1:nbSamples for n=1:nbSamples
%Computation of the resulting Gaussians (for display purpose) %Computation of the resulting Gaussians (for display purpose)
for i=1:model.nbStates for i=1:model.nbStates
model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (48)
end end
r(n).Mu = model.Mu; r(n).Mu = model.Mu;
% %Retrieval of attractor path through GMR % %Retrieval of attractor path through GMR
% currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf % currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
% %Motion retrieval with spring-damper system % %Motion retrieval with spring-damper system
% x = s(n).p(1).b(2:model.nbVar); % x = s(n).p(1).b(2:model.nbVar);
% dx = zeros(nbVarOut,1); % dx = zeros(nbVarOut,1);
% for t=1:s(n).nbData % for t=1:s(n).nbData
% %Compute acceleration, velocity and position % %Compute acceleration, velocity and position
% ddx = -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf % ddx = -L * [x-currTar(:,t); dx];
% dx = dx + ddx * model.dt; % dx = dx + ddx * model.dt;
% x = x + dx * model.dt; % x = x + dx * model.dt;
% r(n).Data(:,t) = x; % r(n).Data(:,t) = x;
...@@ -110,6 +126,7 @@ for n=1:nbRepros ...@@ -110,6 +126,7 @@ for n=1:nbRepros
% %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf % %Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
% rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1]; % rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1];
%Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf %Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1]; rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1];
...@@ -152,9 +169,10 @@ for n=1:nbSamples ...@@ -152,9 +169,10 @@ for n=1:nbSamples
plotGMM(r(n).Mu(2:3,:),model.Sigma(2:3,2:3,:), [0 0 0], .04); plotGMM(r(n).Mu(2:3,:),model.Sigma(2:3,2:3,:), [0 0 0], .04);
end end
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png'); %print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png');
%Plot reproductions in new situations %Plot reproductions in new situations
disp('[Press enter to see next reproduction attempt]');
h=[]; h=[];
for n=1:nbRepros for n=1:nbRepros
delete(h); delete(h);
...@@ -165,8 +183,8 @@ for n=1:nbRepros ...@@ -165,8 +183,8 @@ for n=1:nbRepros
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)]; [1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])]; h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']); %print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
%pause pause;
end end
pause;