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

Initial commit

parent 703a5e8a
# pbdlib-matlab
PbDLib is a set of tools combining statistical learning, dynamical systems and optimal control approaches for programming-by-demonstration applications.
The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, http://calinon.ch.
More information can be found on: http://programming-by-demonstration.org/lib/
### Compatibility
The codes have been tested with both Matlab and GNU Octave.
The codes are compatible with both Matlab or GNU Octave.
### Usage
Examples starting with 'demo_' can be run from Matlab/GNU Octave.
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.
Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention
control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
### Description
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 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
Sylvain Calinon
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
academic publication that uses this code or part of it:
@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"
}
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.",
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"
}
```
### 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
%Benchmark of task-parameterized model based on Gaussian process regression,
%with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction
%Sylvain Calinon, 2015
% Benchmark of task-parameterized model based on Gaussian process regression,
% with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction
%
% 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.nbStates = 3; %Number of Gaussians in the GMM
......@@ -14,8 +26,8 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated
nbVarOut = model.nbVar-1;
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
nbVarOut = model.nbVar-1; %(here: x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
%% 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
% 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 (5).
% 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/DataLQR01.mat');
......@@ -40,7 +52,7 @@ 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 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 = s(1).Data0(1,:);
for n=1:nbSamples
......@@ -102,14 +114,14 @@ for n=1:nbSamples
end
% %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
% x = s(n).p(1).b(2:model.nbVar);
% dx = zeros(nbVarOut,1);
% for t=1:s(n).nbData
% %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;
% x = x + dx * model.dt;
% r(n).Data(:,t) = x;
......@@ -146,14 +158,14 @@ for n=1:nbRepros
end
%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
x = rnew(n).p(1).b(2:model.nbVar);
dx = zeros(nbVarOut,1);
for t=1:nbD
%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;
x = x + dx * model.dt;
rnew(n).Data(:,t) = x;
......@@ -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);
end
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
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
......@@ -193,8 +206,8 @@ for n=1:nbRepros
[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])];
axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
%pause
%print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
pause
end
pause;
......
function benchmark_DS_GP_raw01
%Benchmark of task-parameterized model based on Gaussian process regression,
%with raw trajectory, and spring-damper system used for reproduction
%Sylvain Calinon, 2015
% Benchmark of task-parameterized model based on Gaussian process regression,
% with raw trajectory, and spring-damper system used for reproduction
%
% 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
......@@ -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
L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV];
%% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
% 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 (5).
% 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/DataLQR01.mat');
......@@ -36,7 +49,7 @@ 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(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 = s(1).Data0(1,:);
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)
end
%% GPR with raw trajectory encoding
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of GPR with raw trajectory encoding:');
......@@ -70,7 +84,7 @@ end
% dx = zeros(model.nbVar,1);
% for t=1:s(n).nbData
% %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;
% x = x + dx * model.dt;
% r(n).Data(:,t) = x;
......@@ -101,7 +115,7 @@ for n=1:nbRepros
dx = zeros(model.nbVar,1);
for t=1:nbD
%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;
x = x + dx * model.dt;
rnew(n).Data(:,t) = x;
......@@ -125,9 +139,10 @@ for n=1:nbSamples
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
end
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
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
......@@ -137,8 +152,8 @@ for n=1:nbRepros
[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])];
axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']);
%pause
%print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']);
pause;
end
pause;
......
function benchmark_DS_PGMM01
%Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction
%Sylvain Calinon, 2015
% Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction
%
% 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.nbStates = 3; %Number of Gaussians in the GMM
......@@ -13,8 +25,8 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated
nbVarOut = model.nbVar - 1;
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
nbVarOut = model.nbVar-1; %(here, x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
%% 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
% 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 (5).
% 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/DataLQR01.mat');
......@@ -38,7 +50,7 @@ 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 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
%Compute derivatives
Data = [];
for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:);
......@@ -51,9 +63,10 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of PGMM with EM:');
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];
%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];
end
......@@ -61,10 +74,12 @@ end
% %model = init_GMM_kmeans(Data, model);
% model = init_GMM_timeBased(Data, model);
% model = EM_GMM(Data, model);
for i=1:model.nbStates
% %Initialization of parameters based on standard GMM
% model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1));
% model.ZMu(:,end,i) = model.Mu(:,i);
%Random initialization of parameters
model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1));
model.Sigma(:,:,i) = eye(model.nbVar);
......@@ -83,17 +98,18 @@ nbVarOut = model.nbVar-1;
for n=1:nbSamples
%Computation of the resulting Gaussians (for display purpose)
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
r(n).Mu = model.Mu;
% %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
% x = s(n).p(1).b(2:model.nbVar);
% dx = zeros(nbVarOut,1);
% for t=1:s(n).nbData
% %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;
% x = x + dx * model.dt;
% r(n).Data(:,t) = x;
......@@ -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
% 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
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
plotGMM(r(n).Mu(2:3,:),model.Sigma(2:3,2:3,:), [0 0 0], .04);
end
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
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
......@@ -165,8 +183,8 @@ for n=1:nbRepros
[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])];
axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
%pause
%print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
pause;
end
pause;
......
function benchmark_DS_TP_GMM01
%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
%Sylvain Calinon, 2015
% Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
%
% 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/');
......@@ -22,8 +33,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
% 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 (5).
% 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/DataLQR01.mat');
......@@ -37,7 +48,7 @@ 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 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
%Create 3rd order tensor data with XHAT instead of X
Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:);
......@@ -97,9 +108,10 @@ for n=1:nbSamples
plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
end
axis equal; axis(limAxes);
print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png');
%print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png');
%Plot reproductions in new situations
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
......@@ -110,8 +122,8 @@ for n=1:nbRepros
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']);
%pause
%print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']);
pause;
end
pause;
......
function benchmark_DS_TP_GP01
%Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method)
%Sylvain Calinon, 2015
% Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method)
%
% 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
......@@ -12,8 +24,8 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated
nbVarOut = model.nbVar-1;
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
nbVarOut = model.nbVar-1; %(here, x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
%% Load 3rd order tensor data
......@@ -23,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
% 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 (5).
% 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/DataLQR01.mat');
......@@ -37,7 +49,7 @@ 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(model.nbVar-1));
%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
%Create 3rd order tensor data with XHAT instead of X
Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:);
......@@ -64,6 +76,7 @@ for m=1:model.nbFrames
model.Mu(:,m,:) = MuTmp;
model.Sigma(:,:,m,:) = SigmaTmp;
end
%Reproduction with spring-damper system
% for n=1:nbSamples
% currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
......@@ -121,9 +134,10 @@ for n=1:nbSamples
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
end
axis equal; axis(limAxes);
print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png');
%print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png');
%Plot reproductions in new situations
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
......@@ -133,8 +147,8 @@ for n=1:nbRepros
[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])];
axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
%pause
%print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
pause
end
pause;
......
function benchmark_DS_TP_LWR01
%Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method)
%Sylvain Calinon, 2015
% Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method)
%
% 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
......@@ -12,8 +24,8 @@ model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
nbRepros = 4; %Number of reproductions with new situations randomly generated
nbVarOut = model.nbVar-1;
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
nbVarOut = model.nbVar-1; %(here, x1,x2)
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
%% Load 3rd order tensor data
......@@ -23,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
% 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 (5).
% 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/DataLQR01.mat');
......@@ -37,7 +49,7 @@ 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(model.nbVar-1));
%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);
for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:);
......@@ -75,6 +87,7 @@ for m=1:model.nbFrames
model.Mu(:,m,:) = MuTmp;
model.Sigma(:,:,m,:) = SigmaTmp;
end
%Reproduction with spring-damper system
% for n=1:nbSamples
% currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
......@@ -132,9 +145,10 @@ for n=1:nbSamples
[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
end
axis equal; axis(limAxes);
print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png');
%print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png');
%Plot reproductions in new situations
disp('[Press enter to see next reproduction attempt]');
h=[];
for n=1:nbRepros
delete(h);
......@@ -144,8 +158,8 @@ for n=1:nbRepros
[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])];
axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']);
%pause
%print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']);
pause;
end
pause;
......
function benchmark_DS_TP_MFA01
%Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction
%Sylvain Calinon, 2015
% Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction
%
% 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.nbStates = 3; %Number of Gaussians in the GMM
......@@ -98,9 +110,10 @@ for n=1:nbSamples
plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
end