Commit 8d6dbeee authored by Sylvain CALINON's avatar Sylvain CALINON

PbDlib v1.0

parent bd713a21
This diff is collapsed.
...@@ -2,11 +2,7 @@ function benchmark_DS_GP_GMM01 ...@@ -2,11 +2,7 @@ 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.
% %
% Writing code takes time. Polishing it and making it available to others takes longer! % If this code is useful for your research, please cite the related publication:
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST, % @article{Calinon16JIST,
% author="Calinon, S.", % author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
...@@ -36,8 +32,7 @@ function benchmark_DS_GP_GMM01 ...@@ -36,8 +32,7 @@ function benchmark_DS_GP_GMM01
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>. % along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./../m_fcts/');
addpath('./m_fcts/');
%% Parameters %% Parameters
...@@ -137,7 +132,7 @@ for n=1:nbSamples ...@@ -137,7 +132,7 @@ 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. (17)-(19) % currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]);
% %
% %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);
...@@ -181,7 +176,7 @@ for n=1:nbRepros ...@@ -181,7 +176,7 @@ 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. (17)-(19) [rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]);
%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);
...@@ -230,27 +225,23 @@ for n=1:nbRepros ...@@ -230,27 +225,23 @@ for n=1:nbRepros
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;
close all; close all;
end end
%Function to plot pegs %Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa) function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs') if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078]; colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4; fa = 0.4;
end end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1; pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p) for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2)); dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa); h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end end
end end
\ No newline at end of file
...@@ -2,11 +2,7 @@ function benchmark_DS_GP_raw01 ...@@ -2,11 +2,7 @@ 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.
% %
% Writing code takes time. Polishing it and making it available to others takes longer! % If this code is useful for your research, please cite the related publication:
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST, % @article{Calinon16JIST,
% author="Calinon, S.", % author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
...@@ -36,7 +32,7 @@ function benchmark_DS_GP_raw01 ...@@ -36,7 +32,7 @@ function benchmark_DS_GP_raw01
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>. % along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/'); addpath('./../m_fcts/');
%% Parameters %% Parameters
...@@ -180,22 +176,18 @@ end ...@@ -180,22 +176,18 @@ end
pause; pause;
close all; close all;
end end
%Function to plot pegs %Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa) function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs') if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078]; colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4; fa = 0.4;
end end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1; pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p) for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2)); dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa); h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end end
end end
\ No newline at end of file
function benchmark_DS_PGMM01 function benchmark_DS_PGMM01
% Benchmark of task-parameterized model based on parametric Gaussianvmixture model, % Benchmark of task-parameterized model based on parametric Gaussian mixture model,
% and DS-GMR used for reproduction. % and DS-GMR used for reproduction.
% %
% Writing code takes time. Polishing it and making it available to others takes longer! % If this code is useful for your research, please cite the related publication:
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST, % @article{Calinon16JIST,
% author="Calinon, S.", % author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
...@@ -36,7 +32,7 @@ function benchmark_DS_PGMM01 ...@@ -36,7 +32,7 @@ function benchmark_DS_PGMM01
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>. % along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/'); addpath('./../m_fcts/');
%% Parameters %% Parameters
...@@ -86,10 +82,10 @@ end ...@@ -86,10 +82,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. (48) % %Task parameters rearranged as a vector (position and orientation)
% 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. (48) %Task parameters rearranged as a vector (position only)
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
...@@ -121,12 +117,12 @@ nbVarOut = model.nbVar-1; ...@@ -121,12 +117,12 @@ 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. (48) model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable
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. (17)-(19) % currTar = GMR(model, DataIn, 1, [2:model.nbVar]);
% %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);
...@@ -147,25 +143,25 @@ load('data/taskParams.mat'); %Load new task parameters (new situation) ...@@ -147,25 +143,25 @@ load('data/taskParams.mat'); %Load new task parameters (new situation)
for n=1:nbRepros for n=1:nbRepros
rnew(n).p = taskParams(n).p; rnew(n).p = taskParams(n).p;
% %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)
% 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)
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];
%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) * rnew(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable
end end
rnew(n).Mu = model.Mu; rnew(n).Mu = model.Mu;
%Retrieval of attractor path through GMR %Retrieval of attractor path through GMR
[rnew(n).currTar, rnew(n).currSigma] = GMR(model, 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(model, DataIn, 1, [2:model.nbVar]);
%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;
...@@ -212,22 +208,18 @@ end ...@@ -212,22 +208,18 @@ end
pause; pause;
close all; close all;
end end
%Function to plot pegs %Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa) function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs') if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078]; colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4; fa = 0.4;
end end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1; pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p) for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2)); dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa); h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end end
end end
\ No newline at end of file
...@@ -2,11 +2,7 @@ function benchmark_DS_TP_GMM01 ...@@ -2,11 +2,7 @@ function benchmark_DS_TP_GMM01
% Benchmark of task-parameterized Gaussian mixture model (TP-GMM), % Benchmark of task-parameterized Gaussian mixture model (TP-GMM),
% with DS-GMR used for reproduction. % with DS-GMR used for reproduction.
% %
% Writing code takes time. Polishing it and making it available to others takes longer! % If this code is useful for your research, please cite the related publication:
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST, % @article{Calinon16JIST,
% author="Calinon, S.", % author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
...@@ -36,7 +32,8 @@ function benchmark_DS_TP_GMM01 ...@@ -36,7 +32,8 @@ function benchmark_DS_TP_GMM01
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>. % along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/'); addpath('./../m_fcts/');
%% Parameters %% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
...@@ -151,22 +148,18 @@ end ...@@ -151,22 +148,18 @@ end
pause; pause;
close all; close all;
end end
%Function to plot pegs %Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa) function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs') if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078]; colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4; fa = 0.4;
end end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1; pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p) for m=1:length(p)
dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2)); dispMesh = p(m).A(2:3,2:3) * pegMesh + repmat(p(m).b(2:3),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa); h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end end
end end
\ No newline at end of file
function benchmark_DS_TP_GP01 function benchmark_DS_TP_GP01
% Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method). % Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method).
% %
% Writing code takes time. Polishing it and making it available to others takes longer! % If this code is useful for your research, please cite the related publication:
% If some parts of the code were useful for your research of for a better understanding
% of the algorithms, please reward the authors by citing the related publications,
% and consider making your own research available in this way.
%
% @article{Calinon16JIST, % @article{Calinon16JIST,
% author="Calinon, S.", % author="Calinon, S.",
% title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
...@@ -35,7 +31,7 @@ function benchmark_DS_TP_GP01 ...@@ -35,7 +31,7 @@ function benchmark_DS_TP_GP01
% You should have received a copy of the GNU General Public License % You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>. % along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/'); addpath('./../m_fcts/');
%% Parameters %% Parameters
...@@ -101,14 +97,14 @@ end ...@@ -101,14 +97,14 @@ end
%Reproduction with spring-damper system %Reproduction with spring-damper system
% for n=1:nbSamples % for n=1:nbSamples
% currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf % currTar = productTPGMM0(model, s(n).p);
% %
% %Motion retrieval with spring-damper system % %Motion retrieval with spring-damper system
% x = s(n).p(1).b; % x = s(n).p(1).b;
% 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;
...@@ -125,14 +121,14 @@ for n=1:nbRepros ...@@ -125,14 +121,14 @@ for n=1:nbRepros
rnew(n).p(m).b = taskParams(n).p(m).b(2:end); rnew(n).p(m).b = taskParams(n).p(m).b(2:end);
rnew(n).p(m).A = taskParams(n).p(m).A(2:end,2:end); rnew(n).p(m).A = taskParams(n).p(m).A(2:end,2:end);
end end
[rnew(n).currTar, rnew(n).currSigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf [rnew(n).currTar, rnew(n).currSigma] = productTPGMM0(model, rnew(n).p);
%Motion retrieval with spring-damper system %Motion retrieval with spring-damper system
x = rnew(n).p(1).b; x = rnew(n).p(1).b;
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;
...@@ -170,27 +166,23 @@ for n=1:nbRepros ...@@ -170,27 +166,23 @@ for n=1:nbRepros
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_TP_GP' num2str(n+1,'%.2d') '.png']); %print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
pause pause;
end end
pause; pause;
close all; close all;
end end
%Function to plot pegs %Function to plot pegs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = plotPegs(p, colPegs, fa) function h = plotPegs(p, colPegs, fa)
if ~exist('colPegs') if ~exist('colPegs')
colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078]; colPegs = [0.2863 0.0392 0.2392; 0.9137 0.4980 0.0078];
fa = 0.4; fa = 0.4;
end end
pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1; pegMesh = [-4 -3.5; -4 10; -1.5 10; -1.5 -1; 1.5 -1; 1.5 10; 4 10; 4 -3.5; -4 -3.5]' *1E-1;
for m=1:length(p) for m=1:length(p)
dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2)); dispMesh = p(m).A(1:2,1:2) * pegMesh + repmat(p(m).b(1:2),1,size(pegMesh,2));
h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa); h(m) = patch(dispMesh(1,:),dispMesh(2,:),colPegs(m,:),'linewidth',1,'edgecolor','none','facealpha',fa);
end end
end end
\ No newline at end of file