Commit ffbedaaf authored by Sylvain Calinon's avatar Sylvain Calinon

New examples added

parent 5c35d061
......@@ -53,34 +53,46 @@ All the examples are located in the main folder, and the functions are located i
| 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_affineTransform01 | Miscellaneous affine transformations of raw data as pre-processing step to train a task-parameterized model |
| demo_affineTransform01 | Affine transformations of raw data as pre-processing step to train a task-parameterized model |
| 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_batchLQR02 | Same as demo_batchLQR01 but with only position data |
| demo_DMP_GMR01 | Emulation of a standard dynamic movement primitive (DMP) by using a GMM with diagonal covariance matrix, and retrieval computed through Gaussian mixture regression (GMR) |
| demo_DMP_GMR02 | Same as demo_DMP_GMR01 but with full covariance matrices coordinating the different variables |
| demo_DMP_GMR03 | Same as demo_DMP_GMR02 but with GMR used to regenerate the path of a spring-damper system instead of encoding the nonlinear forcing term |
| demo_DMP_GMR04 | Same as demo_DMP_GMR03 by using the task-parameterized model formalism |
| demo_DMP_GMR_LQR01 | Same as demo_DMP_GMR04 but with LQR used to refine the parameters of the spring-damper system |
| demo_DMP_GMR_LQR02 | Same as demo_DMP_GMR_LQR01 with perturbations added to show the benefit of full covariance to coordinate disturbance rejection |
| 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_GMR01 | GMM and time-based Gaussian mixture regression (GMR) used for reproduction |
| demo_GPR01 | Use of Gaussian process regression (GPR) as a task-parameterized model, with DS-GMR used to retrieve continuous movements |
| demo_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_HDDC01 | High Dimensional Data Clustering (HDDC, or HD-GMM) |
| demo_iterativeLQR01 | Controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, unconstrained linear MPC), by relying on a GMM encoding of position and velocity data (see also demo_batchLQR01) |
| demo_iterativeLQR02 | Same as demo_iterativeLQR01 with only position data |
| demo_MFA01 | Mixture of factor analyzers (MFA) |
| demo_MPPCA01 | Mixture of probabilistic principal component analyzers (MPPCA) |
| demo_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_testDampingRatio01 | Test with critically damped system and ideal underdamped system |
| demo_testLQR01 | Test of linear quadratic regulation (LQR) with different variance in the data |
| demo_testLQR02 | Test of LQR with evaluation of the damping ratio found by the system |
| demo_testLQR03 | Comparison of LQR with finite and infinite time horizons |
| demo_testLQR04 | Demonstration of the coordination capability of linear quadratic optimal control (unconstrained linear MPC) when combined with full precision matrices |
| 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_TPbatchLQR02 | Batch solution of a linear quadratic optimal control 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_TPGMR01 | 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_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_TPMPC01 | Task-parameterized model encoding position data, with MPC used to track the associated stepwise reference path |
| demo_TPMPC02 | Same as demo_TPMPC01 with a generalized version of MPC used to track associated stepwise reference paths in multiple frames |
| demo_TPMPPCA01 | Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) |
| demo_TPtrajGMM01 | Task-parameterized model with trajectory-GMM encoding |
| demo_trajGMM01 | Reproduction of trajectory with a GMM with dynamic features (trajectory GMM) |
| demo_trajGMM01 | Reproduction of trajectory with a GMM with dynamic features (trajectory-GMM) |
| demo_trajMFA01 | Trajectory model with either a mixture of factor analysers (MFA), a mixture of probabilistic principal component analyzers (MPPCA), or a high-dimensional data clustering approach (HD-GMM) |
| demoIK_nullspace_TPGMM01 | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) |
| demoIK_pointing_TPGMM01 | Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), by using a single Euler orientation angle and 3 DOFs robot) |
function demo_DMP_GMR01
%Emulation of a standard dynamic movement primitive (DMP) model by using a Gaussian mixture model (GMM)
%with diagonal convariance matrix, and retrieval computed through Gaussian mixture regression (GMR)
%Sylvain Calinon, 2015
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of activation functions (i.e., number of states in the GMM)
model.nbVar = 3; %Number of variables [s,F1,F2] (decay term and perturbing force)
model.nbVarPos = model.nbVar-1; %Dimension of spatial variables
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
nbData = 200; %Length of each trajectory
nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%% Load AMARSI 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')
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)
end
xTar = demos{1}.pos(:,end);
Data=[];
DataDMP=[];
for n=1:nbSamples
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
Data = [Data s(n).Data]; %Concatenation of the multiple demonstrations
%Training data as [s;F]
DataDMP = [DataDMP [sIn; ...
(s(n).Data(accId,:) - (repmat(xTar,1,nbData)-s(n).Data(posId,:))*model.kP + s(n).Data(velId,:)*model.kV) ./ repmat(sIn,model.nbVarPos,1)]];
end
%% Setting of the basis functions and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_timeBased(DataDMP, model);
%model = init_GMM_logBased(DataDMP, model); %Log-spread in s <-> equal spread in t
%model = init_GMM_kmeans(DataDMP, model);
%Set Sigma as diagonal matrices (i.e., inpedendent systems synchronized by the s variable)
for i=1:model.nbStates
%model.Sigma(:,:,i) = diag(diag(model.Sigma(:,:,i))) * 5E-1;
model.Sigma(:,:,i) = eye(model.nbVar) * 1E-2; %Heuristic setting of covariance
end
%Nonlinear force profile retrieval
currF = GMR(model, sIn, 1, 2:model.nbVar);
%Motion retrieval with DMP
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
%Compute acceleration, velocity and position
ddx = L * [xTar-x; -dx] + currF(:,t) * sIn(t); %See Eq. (4.0.1) in doc/TechnicalReport.pdf
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,450],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Activation of the basis functions
for i=1:model.nbStates
h(i,:) = model.Priors(i) * gaussPDF(sIn, model.Mu(1,i), model.Sigma(1,1,i));
end
h = h ./ repmat(sum(h,1)+realmin, model.nbStates, 1);
%Spatial plot
subplot(2,4,[1,5]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).Data(1,:),r(1).Data(2,:),'-','linewidth',3,'color',[.8 0 0]);
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]);
end
for i=1:model.nbStates
plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,i), clrmap(i,:), .7);
end
plot(sIn, currF(1,:), '-','linewidth',2,'color',[.8 0 0]);
axis([0 1 min(DataDMP(2,:)) max(DataDMP(2,:))]);
ylabel('$F_1$','fontsize',16,'interpreter','latex');
view(180,-90);
%Timeline plot of the basis functions activation
subplot(2,4,[6:8]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, h(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, h(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([0 1 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP_GMR01.png');
pause;
close all;
function demo_DMP_GMR02
%Example of enhanced dynamic movement primitive (DMP) model trained with EM by using a Gaussian mixture model (GMM) representation,
%with full covariance matrices coordinating the different variables in the feature space. After learning (i.e., autonomous organization
%of the basis functions (position and spread), Gaussian mixture regression (GMR) is used to regenerate the nonlinear force profile.
%Sylvain Calinon, 2015
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of states in the GMM
model.nbVar = 3; %Number of variables [s,F1,F2] (decay term and perturbing force)
model.nbVarPos = model.nbVar-1; %Dimension of spatial variables
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
nbData = 200; %Length of each trajectory
nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%% Load AMARSI 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');
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)
end
xTar = demos{1}.pos(:,end);
Data=[];
DataDMP=[];
for n=1:nbSamples
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
Data = [Data s(n).Data]; %Concatenation of the multiple demonstrations
%Training data as [s;F]
DataDMP = [DataDMP [sIn; ...
(s(n).Data(accId,:) - (repmat(xTar,1,nbData)-s(n).Data(posId,:))*model.kP + s(n).Data(velId,:)*model.kV) ./ repmat(sIn,model.nbVarPos,1)]];
end
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%model = init_GMM_timeBased(DataDMP, model);
%model = init_GMM_logBased(DataDMP, model); %Log-spread in s <-> equal spread in t
model = init_GMM_kmeans(DataDMP, model);
model = EM_GMM(DataDMP, model);
%Nonlinear force profile retrieval
currF = GMR(model, sIn, 1, 2:model.nbVar);
%Motion retrieval with DMP
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
%Compute acceleration, velocity and position
ddx = L * [xTar-x; -dx] + currF(:,t) * sIn(t); %See Eq. (4.0.1) in doc/TechnicalReport.pdf
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,450],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Activation of the basis functions
for i=1:model.nbStates
h(i,:) = model.Priors(i) * gaussPDF(sIn, model.Mu(1,i), model.Sigma(1,1,i));
end
h = h ./ repmat(sum(h,1)+realmin, model.nbStates, 1);
%Spatial plot
subplot(2,4,[1,5]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).Data(1,:),r(1).Data(2,:),'-','linewidth',3,'color',[.8 0 0]);
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]);
end
for i=1:model.nbStates
plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,i), clrmap(i,:), .7);
end
plot(sIn, currF(1,:), '-','linewidth',2,'color',[.8 0 0]);
axis([0 1 min(DataDMP(2,:)) max(DataDMP(2,:))]);
ylabel('$F_1$','fontsize',16,'interpreter','latex');
view(180,-90);
%Timeline plot of the basis functions activation
subplot(2,4,[6:8]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, h(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, h(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([0 1 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP_GMR02.png');
pause;
close all;
function demo_DMP_GMR03
%Example of enhanced dynamic movement primitive (DMP) model trained with EM by using a Gaussian mixture model (GMM) representation,
%with full covariance matrices coordinating the different variables in the feature space. After learning (i.e., autonomous organization
%of the basis functions (position and spread), Gaussian mixture regression (GMR) is used to regenerate the path of a spring-damper system,
%resulting in a nonlinear force profile.
%Sylvain Calinon, 2015
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of states in the GMM
model.nbVar = 3; %Number of variables [s,F1,F2] (decay term and perturbing force)
model.nbVarPos = model.nbVar-1; %Dimension of spatial variables
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
nbData = 200; %Length of each trajectory
nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%Create transformation matrix to compute r(1).currTar = x + dx*kV/kP + ddx/kP, see Eq. (4.0.2) in doc/TechnicalReport.pdf
K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.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)
end
Data=[];
DataDMP=[];
for n=1:nbSamples
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
DataDMP = [DataDMP [sIn; K*s(n).Data]]; %Training data as [s;r(1).currTar]
Data = [Data s(n).Data]; %Original data as [x;dx;ddx]
end
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%model = init_GMM_timeBased(DataDMP, model);
%model = init_GMM_logBased(DataDMP, model); %Log-spread in s <-> equal spread in t
model = init_GMM_kmeans(DataDMP, model);
model = EM_GMM(DataDMP, model);
%Spring-damper attractor path retrieval
r(1).currTar = GMR(model, sIn, 1, 2:model.nbVar);
%Motion retrieval with DMP
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
%Compute acceleration, velocity and position
ddx = L * [r(1).currTar(:,t)-x; -dx]; %Spring-damper system
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,450],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Activation of the basis functions
for i=1:model.nbStates
h(i,:) = model.Priors(i) * gaussPDF(sIn, model.Mu(1,i), model.Sigma(1,1,i));
end
h = h ./ repmat(sum(h,1)+realmin, model.nbStates, 1);
%Spatial plot
subplot(2,4,[1,5]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).Data(1,:),r(1).Data(2,:),'-','linewidth',3,'color',[.8 0 0]);
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]);
end
for i=1:model.nbStates
plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,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');
view(180,-90);
%Timeline plot of the basis functions activation
subplot(2,4,[6:8]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, h(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, h(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([0 1 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP_GMR03.png');
pause;
close all;
function demo_DMP_GMR04
%Example of enhanced dynamic movement primitive (DMP) model trained with EM by using a Gaussian mixture model (GMM) representation,
%with full covariance matrices coordinating the different variables in the feature space, and by using the task-parameterized model
%formalism. After learning (i.e., autonomous organization of the basis functions (position and spread), Gaussian mixture
%regression (GMR) is used to regenerate the path of a spring-damper system, resulting in a nonlinear force profile.
%Sylvain Calinon, 2015
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of states in the GMM
model.nbVar = 3; %Number of variables [s,F1,F2] (decay term and perturbing force)
model.nbFrames = 1; %Number of candidate frames of reference (centered on goal position)
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
model.nbVarPos = model.nbVar-1; %Dimension of spatial variables
nbData = 200; %Length of each trajectory
nbSamples = 4; %Number of demonstrations
L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback term
%Create transformation matrix to compute r(1).currTar = x + dx*kV/kP + ddx/kP, see Eq. (4.0.2) in doc/TechnicalReport.pdf
K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.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)
end
DataDMP = zeros(model.nbVar,1,nbData*nbSamples);
Data=[];
for n=1:nbSamples
%Task parameters (canonical coordinate system centered on the end-trajectory target)
s(n).p(1).A = eye(model.nbVar);
s(n).p(1).b = [0; demos{n}.pos(:,end)];
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
Data = [Data s(n).Data]; %Original data
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
s(n).Data = [sIn; K*s(n).Data]; %r(1).currTar computation
s(n).Data = s(n).p(1).A \ (s(n).Data - repmat(s(n).p(1).b,1,nbData)); %Observation from the perspective of the frame
DataDMP(:,1,(n-1)*nbData+1:n*nbData) = s(n).Data; %Training data as [s;r(1).currTar]
end
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%model = init_tensorGMM_kmeans(DataDMP, model);
model = init_tensorGMM_timeBased(DataDMP, model);
model = EM_tensorGMM(DataDMP, model);
%Task-adaptive spring-damper attractor path retrieval
r(n).p(1).A = eye(model.nbVar);
r(n).p(1).b = s(n).p(1).b + [0; 0; 5]; %Offset added to test generalization capability
[r(n).Mu, r(n).Sigma] = productTPGMM0(model, r(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
r(n).Priors = model.Priors;
r(n).nbStates = model.nbStates;
r(1).currTar = GMR(r(n), sIn, 1, 2:model.nbVar); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
%Motion retrieval with DMP
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
%Compute acceleration, velocity and position
ddx = L * [r(1).currTar(:,t)-x; -dx]; %Spring-damper system
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,450],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Activation of the basis functions
for i=1:model.nbStates
h(i,:) = model.Priors(i) * gaussPDF(sIn, model.Mu(1,i), model.Sigma(1,1,i));
end
h = h ./ repmat(sum(h,1)+realmin, model.nbStates, 1);
%Spatial plot
subplot(2,4,[1,5]); hold on; axis off;
plot(Data(1,:),Data(2,:),'.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).currTar(1,:), r(1).currTar(2,:), '-','linewidth',2,'color',[1 .7 .7]); %Attractor path
plot(r(1).Data(1,:),r(1).Data(2,:),'-','linewidth',3,'color',[.8 0 0]); %Retrieved path
plot(r(n).p(1).b(2),r(n).p(1).b(3),'k+','linewidth',2,'markersize',12);
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]);
end
for i=1:model.nbStates
plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,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');
view(180,-90);
%Timeline plot of the basis functions activation
subplot(2,4,[6:8]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, h(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, h(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([0 1 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP_GMR04.png');
pause;
close all;
function demo_DMP_GMR_LQR01
%Example of enhanced dynamic movement primitive (DMP) model trained with EM by using a Gaussian mixture model (GMM) representation,
%with full covariance matrices coordinating the different variables in the feature space, and by using the task-parameterized model
%formalism. After learning (i.e., autonomous organization of the basis functions (position and spread), Gaussian mixture
%regression (GMR) is used to regenerate the path of a spring-damper system, resulting in a nonlinear force profile.
%The gains of the spring-damper system are further refined by LQR based on the retrieved covariance information.
%Sylvain Calinon, 2015
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbStates = 5; %Number of states in the GMM
model.nbVar = 3; %Number of variables [s,F1,F2] (decay term and perturbing force)
model.nbFrames = 1; %Number of candidate frames of reference (centered on goal position)
model.kP = 50; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
model.alpha = 1.0; %Decay factor
model.dt = 0.01; %Duration of time step
model.nbVarPos = model.nbVar-1; %Dimension of spatial variables
model.rFactor = 5E-5; %Weighting term for the minimization of control commands in LQR
nbData = 200; %Number of datapoints in a trajectory
nbSamples = 4; %Number of demonstrations
%Canonical system parameters
A = kron([0 1; 0 0], eye(model.nbVarPos)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf
B = kron([0; 1], eye(model.nbVarPos)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf
C = kron([1,0],eye(model.nbVarPos));
%Discretize system (Euler method)
Ad = A*model.dt + eye(size(A));
Bd = B*model.dt;
%Control cost matrix
R = eye(model.nbVar) * model.rFactor;
R = kron(eye(nbData),R);
%Create transformation matrix to compute xhat = x + dx*kV/kP + ddx/kP, see Eq. (4.0.2) in doc/TechnicalReport.pdf
K1d = [1, model.kV/model.kP, 1/model.kP];
K = kron(K1d,eye(model.nbVarPos));
%% Load AMARSI data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/AMARSI/GShape.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)
end
DataDMP = zeros(model.nbVar,1,nbData*nbSamples);
Data=[];
for n=1:nbSamples
%Task parameters (canonical coordinate system centered on the end-trajectory target)
s(n).p(1).A = eye(model.nbVar);
s(n).p(1).b = [0; demos{n}.pos(:,end)];
%Demonstration data as [x;dx;ddx]
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
Data = [Data s(n).Data]; %Original data
s(n).Data = [s(n).Data; gradient(s(n).Data)/model.dt]; %Velocity computation
s(n).Data = [s(n).Data; gradient(s(n).Data(end-model.nbVarPos+1:end,:))/model.dt]; %Acceleration computation
s(n).Data = [sIn; K*s(n).Data]; %xhat computation
s(n).Data = s(n).p(1).A \ (s(n).Data - repmat(s(n).p(1).b,1,nbData)); %Observation from the perspective of the frame
DataDMP(:,1,(n-1)*nbData+1:n*nbData) = s(n).Data; %Training data as [s;xhat]
end
%% Learning and reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%model = init_tensorGMM_kmeans(DataDMP, model);
model = init_tensorGMM_timeBased(DataDMP, model);
model = EM_tensorGMM(DataDMP, model);
%Task-adaptive spring-damper attractor path retrieval
r(n).p(1).A = eye(model.nbVar);
r(n).p(1).b = s(n).p(1).b + [0; 0; 5]; %Offset added to test generalization capability
[r(n).Mu, r(n).Sigma] = productTPGMM0(model, r(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
r(n).Priors = model.Priors;
r(n).nbStates = model.nbStates;
[r(1).currTar, r(1).currSigma] = GMR(r(n), sIn, 1, 2:model.nbVar); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
%LQR tracking (discrete version)
Q = zeros(model.nbVarPos*2, model.nbVarPos*2);
R = eye(model.nbVarPos) * model.rFactor;
P = zeros(model.nbVarPos*2, model.nbVarPos*2, nbData);
P(1:model.nbVarPos,1:model.nbVarPos,end) = inv(r(1).currSigma(:,:,nbData));
for t=nbData-1:-1:1
Q(1:model.nbVarPos,1:model.nbVarPos) = inv(r(1).currSigma(:,:,t));
P(:,:,t) = Q - Ad' * (P(:,:,t+1) * Bd / (Bd' * P(:,:,t+1) * Bd + R) * Bd' * P(:,:,t+1) - P(:,:,t+1)) * Ad;
end
%Motion retrieval
x = Data(1:model.nbVarPos,1);
dx = zeros(model.nbVarPos,1);
for t=1:nbData
K = (Bd' * P(:,:,t) * Bd + R) \ Bd' * P(:,:,t) * Ad;
ddx = K * ([r(1).currTar(:,t); zeros(model.nbVarPos,1)] - [x; dx]);
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
r(1).Data(:,t) = x;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1300,450],'color',[1 1 1]);
xx = round(linspace(1,64,model.nbStates));
clrmap = colormap('jet')*0.5;
clrmap = min(clrmap(xx,:),.9);
%Activation of the basis functions
for i=1:model.nbStates
h(i,:) = model.Priors(i) * gaussPDF(sIn, model.Mu(1,i), model.Sigma(1,1,i));
end
h = h ./ repmat(sum(h,1)+realmin, model.nbStates, 1);
%Spatial plot
subplot(2,4,[1,5]); hold on; axis off;
plot(Data(1,:), Data(2,:), '.','markersize',8,'color',[.7 .7 .7]);
plot(r(1).currTar(1,:), r(1).currTar(2,:), '-','linewidth',2,'color',[1 .7 .7]); %Attractor path
plot(r(1).Data(1,:), r(1).Data(2,:), '-','linewidth',3,'color',[.8 0 0]); %Retrieved path
plot(r(n).p(1).b(2), r(n).p(1).b(3), 'k+','linewidth',2,'markersize',12);
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]);
end
for i=1:model.nbStates
plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,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');
view(180,-90);
%Timeline plot of the basis functions activation
subplot(2,4,[6:8]); hold on;
for i=1:model.nbStates
patch([sIn(1), sIn, sIn(end)], [0, h(i,:), 0], min(clrmap(i,:)+0.5,1), 'EdgeColor', 'none', 'facealpha', .4);
plot(sIn, h(i,:), 'linewidth', 2, 'color', min(clrmap(i,:)+0.2,1));
end
axis([0 1 0 1]);
xlabel('$s$','fontsize',16,'interpreter','latex');
ylabel('$h$','fontsize',16,'interpreter','latex');
view(180,-90);
%print('-dpng','graphs/demo_DMP_GMR_LQR01.png');
pause;
close all;
function demo_DMP_GMR_LQR02