Commit 0f3045f4 authored by Milad Malekzadeh's avatar Milad Malekzadeh

Tab2.

parent 37a729bd
function demo_DSGMR01 function demo_DSGMR01
% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper % Demonstration 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 % 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 % 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 % relevance of the frame through the task. This information is exploited to generate a new attractor path
% corresponding to new situations (new positions and orientation of the frames). % corresponding to new situations (new positions and orientation of the frames).
% %
% This demo presents the results for a dynamical system with constant gains. % This demo presents the results for a dynamical system with constant gains.
% %
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -28,18 +28,18 @@ function demo_DSGMR01 ...@@ -28,18 +28,18 @@ function demo_DSGMR01
model.nbStates = 3; %Number of Gaussians in the GMM model.nbStates = 3; %Number of Gaussians in the GMM
model.nbFrames = 2; %Number of candidate frames of reference model.nbFrames = 2; %Number of candidate frames of reference
model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2) model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
model.dt = 0.01; %Time step model.dt = 0.01; %Time step
model.kP = 100; %Stiffness gain model.kP = 100; %Stiffness gain
model.kV = (2*model.kP)^.5; %Damping gain model.kV = (2*model.kP)^.5; %Damping gain
nbRepros = 8; %Number of reproductions with new situations randomly generated nbRepros = 8; %Number of reproductions with new situations randomly generated
%% Load 3rd order tensor data %% Load 3rd order tensor data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Load 3rd order tensor data...'); disp('Load 3rd order tensor data...');
% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for % The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for
% 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=200x4 the number of datapoints in a trajectory (200)
% multiplied by the number of demonstrations (5). % multiplied by the number of demonstrations (5).
load('data/DataLQR01.mat'); load('data/DataLQR01.mat');
...@@ -60,7 +60,7 @@ for n=1:nbSamples ...@@ -60,7 +60,7 @@ for n=1:nbSamples
DataTmp = s(n).Data0(2:end,:); DataTmp = s(n).Data0(2:end,:);
DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]]; DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
for m=1:model.nbFrames for m=1:model.nbFrames
Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD)); Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
end end
end end
...@@ -68,7 +68,7 @@ end ...@@ -68,7 +68,7 @@ end
%% Tensor GMM learning %% Tensor GMM learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation of tensor GMM with EM:'); fprintf('Parameters estimation of tensor GMM with EM:');
model = init_tensorGMM_timeBased(Data, model); %Initialization model = init_tensorGMM_timeBased(Data, model); %Initialization
model = EM_tensorGMM(Data, model); model = EM_tensorGMM(Data, model);
...@@ -79,16 +79,16 @@ DataIn = [1:s(1).nbData] * model.dt; ...@@ -79,16 +79,16 @@ DataIn = [1:s(1).nbData] * model.dt;
for n=1:nbSamples for n=1:nbSamples
%Retrieval of attractor path through task-parameterized GMR %Retrieval of attractor path through task-parameterized GMR
a(n) = estimateAttractorPath(DataIn, model, s(n)); a(n) = estimateAttractorPath(DataIn, model, s(n));
r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1));
end end
%% Reproduction with DS-GMR for new task parameters %% Reproduction with DS-GMR for new task parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('New reproductions with DS-GMR...'); disp('New reproductions with DS-GMR...');
for n=1:nbRepros for n=1:nbRepros
for m=1:model.nbFrames for m=1:model.nbFrames
%Random generation of new task parameters %Random generation of new task parameters
id=ceil(rand(2,1)*nbSamples); id=ceil(rand(2,1)*nbSamples);
w=rand(2); w=w/sum(w); w=rand(2); w=w/sum(w);
rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2); rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
...@@ -96,7 +96,7 @@ for n=1:nbRepros ...@@ -96,7 +96,7 @@ for n=1:nbRepros
end end
%Retrieval of attractor path through task-parameterized GMR %Retrieval of attractor path through task-parameterized GMR
anew(n) = estimateAttractorPath(DataIn, model, rTmp); anew(n) = estimateAttractorPath(DataIn, model, rTmp);
rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1));
end end
...@@ -161,8 +161,8 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]); ...@@ -161,8 +161,8 @@ axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
%print('-dpng','outTest1.png'); %print('-dpng','outTest1.png');
%Plot additional information %Plot additional information
figure; figure;
%Plot norm of control commands %Plot norm of control commands
subplot(1,2,1); hold on; subplot(1,2,1); hold on;
for n=1:nbRepros for n=1:nbRepros
......
...@@ -4,8 +4,8 @@ function demo_testLQR01 ...@@ -4,8 +4,8 @@ function demo_testLQR01
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -20,16 +20,16 @@ function demo_testLQR01 ...@@ -20,16 +20,16 @@ function demo_testLQR01
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
model.dt = 0.01; %Time step model.dt = 0.01; %Time step
nbData = 500; %Number of datapoints nbData = 500; %Number of datapoints
nbRepros = 3; %Number of reproductions with new situations randomly generated nbRepros = 3; %Number of reproductions with new situations randomly generated
rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
%% Reproduction with LQR %% Reproduction with LQR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...'); disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt; DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData); a.currTar = ones(1,nbData);
for n=1:nbRepros for n=1:nbRepros
a.currSigma = ones(1,1,nbData) * 10^(2-n); a.currSigma = ones(1,1,nbData) * 10^(2-n);
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
...@@ -48,7 +48,7 @@ for n=1:nbRepros ...@@ -48,7 +48,7 @@ for n=1:nbRepros
end end
xlabel('t'); ylabel('x_1'); xlabel('t'); ylabel('x_1');
figure; figure;
%Plot norm of control commands %Plot norm of control commands
subplot(1,2,1); hold on; subplot(1,2,1); hold on;
for n=1:nbRepros for n=1:nbRepros
...@@ -70,8 +70,8 @@ function demo_testLQR01 ...@@ -70,8 +70,8 @@ function demo_testLQR01
% Author: Sylvain Calinon, 2014 % Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon % http://programming-by-demonstration.org/SylvainCalinon
% %
% This source code is given for free! In exchange, I would be grateful if you cite % 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: % the following reference in any academic publication that uses this code or part of it:
% %
% @inproceedings{Calinon14ICRA, % @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.", % author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
...@@ -86,20 +86,20 @@ function demo_testLQR01 ...@@ -86,20 +86,20 @@ function demo_testLQR01
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1) model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
model.dt = 0.01; %Time step model.dt = 0.01; %Time step
nbData = 500; %Number of datapoints nbData = 500; %Number of datapoints
nbRepros = 3; %Number of reproductions with new situations randomly generated nbRepros = 3; %Number of reproductions with new situations randomly generated
rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
%% Reproduction with LQR %% Reproduction with LQR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...'); disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt; DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData); a.currTar = ones(1,nbData);
for n=1:nbRepros for n=1:nbRepros
a.currSigma = ones(1,1,nbData) * 10^(2-n); a.currSigma = ones(1,1,nbData) * 10^(2-n);
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor); %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor); r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
end end
%% Plots %% Plots
...@@ -109,22 +109,22 @@ hold on; box on; ...@@ -109,22 +109,22 @@ hold on; box on;
%Plot target %Plot target
plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2); plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
for n=1:nbRepros for n=1:nbRepros
%Plot trajectories %Plot trajectories
plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
end end
xlabel('t'); ylabel('x_1'); xlabel('t'); ylabel('x_1');
figure; figure;
%Plot norm of control commands %Plot norm of control commands
subplot(1,2,1); hold on; subplot(1,2,1); hold on;
for n=1:nbRepros for n=1:nbRepros
plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
end end
xlabel('t'); ylabel('|ddx|'); xlabel('t'); ylabel('|ddx|');
%Plot stiffness %Plot stiffness
subplot(1,2,2); hold on; subplot(1,2,2); hold on;
for n=1:nbRepros for n=1:nbRepros
plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros); plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
end end
xlabel('t'); ylabel('kp'); xlabel('t'); ylabel('kp');
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment