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

Initial commit

parent 703a5e8a
# pbdlib-matlab # 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 ### Compatibility
The codes have been tested with both Matlab and GNU Octave. The codes are compatible with both Matlab or GNU Octave.
### Usage ### Usage
Examples starting with 'demo_' can be run from Matlab/GNU Octave. Examples starting with `demo_` can be run from Matlab/GNU Octave.
### References ### References
Calinon, S. (in preparation). A tutorial on task-parameterized movement learning and retrieval. Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used some parts of these codes.
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). @article{Calinon15,
author="Calinon, S.",
### Description title="A tutorial on task-parameterized movement learning and retrieval",
year="2015",
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 @inproceedings{Calinon14ICRA,
orientation of the frames), while the predicted covariances are exploited by a linear quadratic regulator (LQR) to author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
estimate the stiffness and damping feedback terms of the spring-damper systems, resulting in a minimal intervention title="A task-parameterized probabilistic model with minimal intervention control",
control strategy. booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
year="2014",
### Datasets month="May-June",
address="Hong Kong, China",
The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within pages="3339--3344"
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 ### Dataset
Sylvain Calinon 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.
http://programming-by-demonstration.org/lib/
### List of examples
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: All the examples are located in the main folder, and the functions are located in the `m_fcts` folder.
@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"
}
| 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; pause;
......
function benchmark_DS_TP_GMM01 function benchmark_DS_TP_GMM01
%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction % Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with 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/');
...@@ -22,8 +33,8 @@ disp('Load 3rd order tensor data...'); ...@@ -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 % 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');
...@@ -37,7 +48,7 @@ D(end,end) = 0; ...@@ -37,7 +48,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 %Create 3rd order tensor data with XHAT instead of X
Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples); Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
for n=1:nbSamples for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:); DataTmp = s(n).Data0(2:end,:);
...@@ -97,9 +108,10 @@ for n=1:nbSamples ...@@ -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); 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_TP_GMM01.png'); %print('-dpng','-r600','graphs/benchmark_DS_TP_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);
...@@ -110,8 +122,8 @@ for n=1:nbRepros ...@@ -110,8 +122,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(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])]; h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])];
axis equal; axis(limAxes); axis equal; axis(limAxes);
print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']); %print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']);
%pause pause;
end end
pause; pause;
......