Skip to content

pbdlib-matlab
pbdlib-matlab
Commit 4522d23a authored by Sylvain Calinon's avatar Sylvain Calinon
Browse files
Options

Use of a common 2D handwriting movement dataset in all examples

parent 37aaafb8
Hide whitespace changes
Inline Side-by-side
Showing with 153 additions and 110 deletions
README.md
View file @ 4522d23a
......@@ -2,17 +2,7 @@
PbDlib is a set of tools combining statistical learning, dynamical systems and optimal control approaches for programming-by-demonstration applications (see http://www.idiap.ch/software/pbdlib/ for details).
The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, Idiap Research Institute.
A C++ version of the library (with currently fewer functionalities) is available at https://gitlab.idiap.ch/rli/pbdlib
### Compatibility
The codes are compatible with both Matlab and GNU Octave.
### Usage
Examples starting with `demo_` can be run from Matlab/GNU Octave.
The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, Idiap Research Institute. A C++ version of the library (with currently fewer functionalities) is available at https://gitlab.idiap.ch/rli/pbdlib
### References
......@@ -39,9 +29,13 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
### Dataset
### Compatibility
The codes are compatible with both Matlab and GNU Octave.
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.
### Usage
Examples starting with `demo_` can be run from Matlab/GNU Octave.
### License
......
demo_DMP_GMR01.m
View file @ 4522d23a
......@@ -48,11 +48,11 @@ nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
posId=[1:model.nbVar-1]; velId=[model.nbVar:2*(model.nbVar-1)]; accId=[2*(model.nbVar-1)+1:3*(model.nbVar-1)];
demos=[];
load('data/AMARSI/GShape.mat')
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
......
demo_DMP_GMR02.m
View file @ 4522d23a
......@@ -50,11 +50,11 @@ nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
posId=[1:model.nbVar-1]; velId=[model.nbVar:2*(model.nbVar-1)]; accId=[2*(model.nbVar-1)+1:3*(model.nbVar-1)];
demos=[];
load('data/AMARSI/GShape.mat');
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
......
demo_DMP_GMR03.m
View file @ 4522d23a
......@@ -54,10 +54,10 @@ K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat');
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
......
demo_DMP_GMR04.m
View file @ 4522d23a
......@@ -56,10 +56,10 @@ K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat');
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
......
demo_DMP_GMR_LQR01.m
View file @ 4522d23a
......@@ -69,10 +69,10 @@ K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat');
load('data/2Dletters/G.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
......
demo_DMP_GMR_LQR02.m
View file @ 4522d23a
......@@ -53,7 +53,7 @@ model.dt = 0.01; %Duration of time step
model.nbVarPos = model.nbVar-1; %Dimension of spatial variables
model.rFactor = 1E-5; %Weighting term for the minimization of control commands in LQR
nbData = 200; %Number of datapoints in a trajectory
nbSamples = 1; %Number of demonstrations
nbSamples = 3; %Number of demonstrations
%Canonical system parameters
A = kron([0 1; 0 0], eye(model.nbVarPos)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf
......@@ -71,10 +71,10 @@ K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/Line.mat');
load('data/2Dletters/I.mat');
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
......@@ -133,21 +133,22 @@ Fpert = zeros(model.nbVarPos,nbData);
%Fpert(:,5) = [4E4; 0]; %Impulse perturbation force at the beginning of the motion
%Motion retrieval
x = Data(1:model.nbVarPos,1) + [-10; 10]; %Offset for the starting point
x = Data(1:model.nbVarPos,1) + [-5; -2]; %Offset for the starting point
dx = zeros(model.nbVarPos,1);
for t=1:nbData
r(1).Data(:,t) = x;
K = (Bd' * P(:,:,t) * Bd + R) \ Bd' * P(:,:,t) * Ad;
ddx = K * ([r(1).currTar(:,t); zeros(model.nbVarPos,1)] - [x; dx]) + Fpert(:,t);
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
r(1).detKp(t) = det(K(:,1:model.nbVarPos));
end
%Motion retrieval
x = Data(1:model.nbVarPos,1) + [-10; 10]; %Offset for the starting point
x = Data(1:model.nbVarPos,1) + [-5; -2]; %Offset for the starting point
dx = zeros(model.nbVarPos,1);
for t=1:nbData
r(2).Data(:,t) = x;
K = (Bd' * P(:,:,t) * Bd + R) \ Bd' * P(:,:,t) * Ad;
%Corresponding scalar stiffness and damping gains (for comparison purpose)
K(:,1:model.nbVarPos) = eye(model.nbVarPos) * det(K(:,1:model.nbVarPos))^(1/model.nbVarPos);
......@@ -156,7 +157,6 @@ for t=1:nbData
ddx = K * ([r(1).currTar(:,t); zeros(model.nbVarPos,1)] - [x; dx]) + Fpert(:,t);
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(2).Data(:,t) = x;
end
......@@ -185,14 +185,14 @@ axis equal;
%Timeline plot of the nonlinear perturbing force
subplot(2,4,[2:4]); hold on;
for n=1:nbSamples
plot(sIn, DataDMP(2,(n-1)*nbData+1:n*nbData), '-','linewidth',2,'color',[.7 .7 .7]);
plot(sIn, DataDMP(3,(n-1)*nbData+1:n*nbData), '-','linewidth',2,'color',[.7 .7 .7]);
end
for i=1:model.nbStates
plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,i), clrmap(i,:), .7);
plotGMM(model.Mu([1,3],i), model.Sigma([1,3],[1,3],i), clrmap(i,:), .7);
end
plot(sIn, r(1).currTar(1,:), '-','linewidth',2,'color',[.8 0 0]);
axis([0 1 min(DataDMP(2,:)) max(DataDMP(2,:))]);
ylabel('$\hat{x}_1$','fontsize',16,'interpreter','latex');
plot(sIn, r(1).currTar(2,:), '-','linewidth',2,'color',[.8 0 0]);
%axis([0 1 min(DataDMP(3,:)) max(DataDMP(3,:))]);
ylabel('$\hat{x}_2$','fontsize',16,'interpreter','latex');
view(180,-90);
%Timeline plot of the evolution of stiffness
......
demo_DSGMR01.m
View file @ 4522d23a
......@@ -48,7 +48,7 @@ nbVarOut = model.nbVar-1; %Dimension of spatial variables
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback term
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sIn(1) = 1; %Initialization of decay term
for t=2:nbData
......@@ -61,7 +61,7 @@ end
% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt;
% D(end,end) = 0;
load('data/AMARSI/GShape.mat');
load('data/2Dletters/G.mat');
Data=[]; Data0=[];
for n=1:nbSamples
DataTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
......
demo_DTW01.m
View file @ 4522d23a
......@@ -41,10 +41,10 @@ nbSamples = 5; %Number of demonstrations
nbVar = 2; %Number of dimensions (max 2 for AMARSI data)
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat');
load('data/2Dletters/G.mat');
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos(1:nbVar,:), linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
end
......
demo_GMM01.m
View file @ 4522d23a
......@@ -38,13 +38,13 @@ addpath('./m_fcts/');
model.nbStates = 5; %Number of states in the GMM
model.nbVar = 2; %Number of variables [x1,x2]
nbData = 200; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%% Load AMARSI data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat');
nbSamples = length(demos);
load('data/2Dletters/G.mat');
%nbSamples = length(demos);
Data=[];
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
......@@ -60,9 +60,9 @@ model = EM_GMM(Data, model);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1000,500]); hold on; box on;
plotGMM(model.Mu, model.Sigma, [.8 0 0]);
figure('position',[10,10,700,500]); hold on; box on;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plotGMM(model.Mu, model.Sigma, [.8 0 0],.5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
%print('-dpng','graphs/demo_GMM01.png');
......
demo_GMR01.m
View file @ 4522d23a
......@@ -43,10 +43,10 @@ nbData = 200; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/CShape.mat');
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
......@@ -64,16 +64,16 @@ model = EM_GMM(Data, model);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1000,500]);
figure('position',[10,10,1300,500]);
%Plot GMM
subplot(1,2,1); hold on; box on; title('GMM');
plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0]);
plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0], .5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
%Plot GMR
subplot(1,2,2); hold on; box on; title('GMR');
plotGMM(DataOut, SigmaOut, [0 .8 0]);
plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
plotGMM(DataOut, SigmaOut, [0 .8 0], .03);
plot(DataOut(1,:),DataOut(2,:),'-','linewidth',2,'color',[0 .4 0]);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
......
demo_HDDC01.m
View file @ 4522d23a
......@@ -52,15 +52,15 @@ nbData = 200; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
load('data/2Dletters/C.mat'); %Load x1,x2 variables
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
end
demos=[];
load('data/AMARSI/CShape.mat'); %Load x3,x4 variables
load('data/2Dletters/D.mat'); %Load x3,x4 variables
Data=[];
for n=1:nbSamples
s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
......@@ -77,12 +77,12 @@ model = EM_HDGMM(Data, model);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1000,500]);
figure('position',[10,10,1300,500]);
for i=1:2
subplot(1,2,i); hold on; box on;
plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8]);
plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0]);
plot(Data((i-1)*2+1,:),Data(i*2,:),'.','markersize',8,'color',[.7 .7 .7]);
plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8], .5);
plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0], .5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
xlabel(['x_' num2str((i-1)*2+1)]); ylabel(['x_' num2str(i*2)]);
end
......
demo_MFA01.m
View file @ 4522d23a
......@@ -42,15 +42,15 @@ nbData = 200; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
load('data/2Dletters/C.mat'); %Load x1,x2 variables
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
end
demos=[];
load('data/AMARSI/CShape.mat'); %Load x3,x4 variables
load('data/2Dletters/D.mat'); %Load x3,x4 variables
Data=[];
for n=1:nbSamples
s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
......@@ -67,12 +67,12 @@ model = EM_MFA(Data, model);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1000,500]);
figure('position',[10,10,1300,500]);
for i=1:2
subplot(1,2,i); hold on; box on;
plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8]);
plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0]);
plot(Data((i-1)*2+1,:),Data(i*2,:),'.','markersize',8,'color',[.7 .7 .7]);
plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8], .5);
plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0], .5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
xlabel(['x_' num2str((i-1)*2+1)]); ylabel(['x_' num2str(i*2)]);
end
......
demo_MPPCA01.m
View file @ 4522d23a
......@@ -42,15 +42,15 @@ nbData = 200; %Length of each trajectory
nbSamples = 5; %Number of demonstrations
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
load('data/2Dletters/C.mat'); %Load x1,x2 variables
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
end
demos=[];
load('data/AMARSI/CShape.mat'); %Load x3,x4 variables
load('data/2Dletters/D.mat'); %Load x3,x4 variables
Data=[];
for n=1:nbSamples
s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
......@@ -67,12 +67,12 @@ model = EM_MPPCA(Data, model);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1000,500]);
figure('position',[10,10,1300,500]);
for i=1:2
subplot(1,2,i); hold on; box on;
plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8]);
plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0]);
plot(Data((i-1)*2+1,:),Data(i*2,:),'.','markersize',8,'color',[.7 .7 .7]);
plotGMM(model0.Mu((i-1)*2+1:i*2,:), model0.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 .8 .8], .5);
plotGMM(model.Mu((i-1)*2+1:i*2,:), model.Sigma((i-1)*2+1:i*2,(i-1)*2+1:i*2,:), [.8 0 0], .5);
axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
xlabel(['x_' num2str((i-1)*2+1)]); ylabel(['x_' num2str(i*2)]);
end
......
demo_batchLQR01.m
View file @ 4522d23a
......@@ -36,7 +36,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbSamples = 3; %Number of demonstrations
nbSamples = 5; %Number of demonstrations
nbRepros = 5; %Number of reproductions
nbData = 100; %Number of datapoints
......@@ -68,9 +68,9 @@ for n=2:nbData
M = [A*M(:,1:model.nbVarPos), M];
end
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/AMARSI/GShape.mat')
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
......
demo_batchLQR02.m
View file @ 4522d23a
......@@ -36,7 +36,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbSamples = 3; %Number of demonstrations
nbSamples = 5; %Number of demonstrations
nbRepros = 5; %Number of reproductions
nbData = 100; %Number of datapoints
......@@ -69,9 +69,9 @@ for n=2:nbData
end
%% Load AMARSI handwriting data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/AMARSI/GShape.mat')
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
......
demo_iterativeLQR01.m
View file @ 4522d23a
......@@ -61,14 +61,13 @@ model.dt = 0.01; %Time step duration
%Dynamical System settings (discrete version)
A = kron([1, model.dt; 0, 1], eye(model.nbVarPos));
B = kron([0; model.dt], eye(model.nbVarPos));
C = kron([1, 0], eye(model.nbVarPos));
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
%% Load Data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/AMARSI/GShape.mat')
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
......@@ -87,7 +86,6 @@ end
%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%model = init_GMM_kmeans(Data,model);
%Initialization based on position data
model0 = init_GMM_kmeans(Data(1:model.nbVarPos,:), model);
[~, GAMMA2] = EM_GMM(Data(1:model.nbVarPos,:), model0);
......@@ -97,10 +95,8 @@ for i=1:model.nbStates
DataTmp = Data - repmat(model.Mu(:,i),1,nbData*nbSamples);
model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
end
%Refinement of parameters
[model, H] = EM_GMM(Data, model);
%Set list of states according to first demonstration (alternatively, an HSMM can be used)
[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1
......@@ -109,36 +105,89 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P = zeros(model.nbVar,model.nbVar,nbData);
P(:,:,end) = inv(model.Sigma(:,:,qList(nbData)));
d = zeros(model.nbVar, nbData);
for t=nbData-1:-1:1
Q = inv(model.Sigma(:,:,qList(t)));
P(:,:,t) = Q - A' * (P(:,:,t+1) * B / (B' * P(:,:,t+1) * B + R) * B' * P(:,:,t+1) - P(:,:,t+1)) * A;
d(:,t) = (A' - A'*P(:,:,t+1) * B / (R + B' * P(:,:,t+1) * B) * B' ) * (P(:,:,t+1) * (A * model.Mu(:,qList(t)) - model.Mu(:,qList(t+1))) + d(:,t+1));
end
%Reproduction
%Reproduction with feedback (FB) and feedforward (FF) terms
for n=1:nbRepros
x = Data(1:model.nbVarPos,1) + randn(model.nbVarPos,1)*2E0;
dx = Data(model.nbVarPos+1:end,1);
X = Data(:,1) + [randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)];
r(n).X0 = X;
for t=1:nbData
K = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A;
ddx = K * (model.Mu(:,qList(t)) - [x; dx]);
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(n).Data(:,t) = [x;dx;ddx];
r(n).Data(:,t) = X; %Log data
K = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A; %FB term
M = -(B' * P(:,:,t) * B + R) \ B' * (P(:,:,t) * (A * model.Mu(:,qList(t)) - model.Mu(:,qList(t))) + d(:,t)); %FF term
u = K * (model.Mu(:,qList(t)) - X) + M; %Acceleration command with FB and FF terms
X = A * X + B * u; %Update of state vector
end
end
%% Batch LQR reproduction (for comparison)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (27)
MuQ = reshape(model.Mu(:,qList), model.nbVar*nbData, 1);
SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,qList), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
%Build Sx and Su matrices for batch LQR, see Eq. (35)
Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
Sx = kron(ones(nbData,1),eye(model.nbVar));
M = B;
for n=2:nbData
id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
Sx(id1,:) = Sx(id1,:) * A;
id1 = (n-1)*model.nbVar+1:n*model.nbVar;
id2 = 1:(n-1)*model.nbVarPos;
Su(id1,id2) = M;
M = [A*M(:,1:model.nbVarPos), M];
end
%Set matrices to compute the damped weighted least squares estimate, see Eq. (37)
SuInvSigmaQ = Su' / SigmaQ;
Rq = SuInvSigmaQ * Su + kron(eye(nbData-1),R);
%Reproductions
for n=1:nbRepros
X = r(n).X0;
rq = SuInvSigmaQ * (MuQ-Sx*X);
u = Rq \ rq;
r2(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5]);
plot(Data(1,:), Data(2,:), 'k.');
figure('position',[10 10 1300 500],'color',[1 1 1]);
%Plot position
subplot(1,2,1); hold on;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
end
for n=1:nbRepros
plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',16,'color',[1 0 0]);
h(1) = plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]); %Reproduction with iterative LQR
h(2) = plot(r2(n).Data(1,:), r2(n).Data(2,:), '--','linewidth',2,'color',[0 .8 0]); %Reproduction with batch LQR
end
axis equal;
xlabel('x_1'); ylabel('x_2');
legend(h,'Iterative LQR','Batch LQR');
%Plot velocity
subplot(1,2,2); hold on;
plotGMM(model.Mu(3:4,:), model.Sigma(3:4,3:4,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(3,(n-1)*nbData+1:n*nbData), Data(4,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
end
for n=1:nbRepros
plot(r(n).Data(3,:), r(n).Data(4,:), '-','linewidth',2,'color',[.8 0 0]); %Reproduction with iterative LQR
plot(r(n).Data(3,1), r(n).Data(4,1), '.','markersize',18,'color',[.6 0 0]);
plot(r2(n).Data(3,:), r2(n).Data(4,:), '--','linewidth',2,'color',[0 .8 0]); %Reproduction with batch LQR
end
plot(0,0,'k+');
axis equal;
xlabel('dx_1'); ylabel('dx_2');
%print('-dpng','graphs/demo_iterativeLQR01.png');
pause;
close all;
%pause;
%close all;
demo_iterativeLQR02.m
View file @ 4522d23a
......@@ -65,9 +65,9 @@ B = kron([0; model.dt], eye(model.nbVarPos));
R = eye(model.nbVarPos) * model.rfactor;
%% Load Data
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('data/AMARSI/GShape.mat')
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
s(n).Data=[];
......@@ -121,7 +121,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 1300 600],'color',[1 1 1]);
figure('position',[10 10 1300 500],'color',[1 1 1]);
%Plot position
subplot(1,2,1); hold on;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
......@@ -149,6 +149,6 @@ axis equal;
xlabel('dx_1'); ylabel('dx_2');
%print('-dpng','graphs/demo_iterativeLQR02.png');
pause;
close all;
%pause;
%close all;
demo_trajGMM01.m
View file @ 4522d23a
......@@ -62,8 +62,8 @@ nbData = 100; %Number of datapoints in a trajectory
% end
% Data = DataTmp;
%Load Amarsi handwriting movements
load('data/AMARSI/Sshape.mat');
%Load handwriting movements
load('data/2Dletters/S.mat');
Data=[];
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
......
demo_trajMFA01.m
View file @ 4522d23a
......@@ -38,27 +38,27 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 3; %Number of components in the mixture model
model.nbStates = 5; %Number of components in the mixture model
model.nbFA = 1; %Dimension of the subspace in each cluster
model.nbVarPos = 4; %Dimension of the position datapoint
model.nbDeriv = 3; %Nb derivatives+1 (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
model.nbVar = model.nbVarPos * model.nbDeriv;
model.dt = 1; %Time step (large values such as 1 will tend to create clusers by following position information)
nbData = 100; %Number of datapoints in a trajectory
nbSamples = 1; %Number of trajectory samples
nbSamples = 5; %Number of trajectory samples
[PHI,PHI1] = constructPHI(model,nbData,nbSamples); %Construct PHI operator (big sparse matrix)
%% Load AMARSI handwriting data
%% Load handwriting data