demo_testLQR01.m 3.45 KB
Newer Older
Milad Malekzadeh's avatar
Milad Malekzadeh committed
1
function demo_testLQR01
2
% Test of the linear quadratic regulation with different variance in the data.
Milad Malekzadeh's avatar
Milad Malekzadeh committed
3
%
4 5 6 7
% Writing code takes time. Polishing it and making it available to others takes longer! 
% 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.
Milad Malekzadeh's avatar
Milad Malekzadeh committed
8
%
9
% @article{Calinon16JIST,
Sylvain Calinon's avatar
Sylvain Calinon committed
10
%   author="Calinon, S.",
11 12
%   title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
%   journal="Intelligent Service Robotics",
13 14 15 16 17 18
%		publisher="Springer Berlin Heidelberg",
%		doi="10.1007/s11370-015-0187-9",
%		year="2016",
%		volume="9",
%		number="1",
%		pages="1--29"
19 20 21 22 23 24 25 26 27 28
% }
%
% @inproceedings{Calinon14,
%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
%   title="A task-parameterized probabilistic model with minimal intervention control",
%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
%   year="2014",
%   month="May-June",
%   address="Hong Kong, China",
%   pages="3339--3344"
Milad Malekzadeh's avatar
Milad Malekzadeh committed
29
% }
30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
% 
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon (http://calinon.ch/) and Danilo Bruno (danilo.bruno@iit.it)
% 
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
% 
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
% 
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
% 
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
Milad Malekzadeh's avatar
Milad Malekzadeh committed
47

48 49
addpath('./m_fcts/');

Sylvain Calinon's avatar
Sylvain Calinon committed
50

Milad Malekzadeh's avatar
Milad Malekzadeh committed
51 52 53
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
54
model.dt = 0.01; %Time step
Milad Malekzadeh's avatar
Milad Malekzadeh committed
55 56 57 58
nbData = 500; %Number of datapoints
nbRepros = 3; %Number of reproductions with new situations randomly generated
rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR

Sylvain Calinon's avatar
Sylvain Calinon committed
59

Milad Malekzadeh's avatar
Milad Malekzadeh committed
60
%% Reproduction with LQR
Milad Malekzadeh's avatar
Milad Malekzadeh committed
61 62 63
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
64
a.currTar = ones(1,nbData);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
65 66
for n=1:nbRepros
	a.currSigma = ones(1,1,nbData) * 10^(2-n);
Sylvain Calinon's avatar
Sylvain Calinon committed
67 68 69
	%r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor);
	r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor);
	r(n).Data = [DataIn; r(n).Data];
Milad Malekzadeh's avatar
Milad Malekzadeh committed
70 71
end

Sylvain Calinon's avatar
Sylvain Calinon committed
72

Milad Malekzadeh's avatar
Milad Malekzadeh committed
73 74 75 76 77 78 79 80 81 82 83 84
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
hold on; box on;
%Plot target
plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
for n=1:nbRepros
	%Plot trajectories
	plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
end
xlabel('t'); ylabel('x_1');

Milad Malekzadeh's avatar
Milad Malekzadeh committed
85
figure;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
86 87 88 89 90 91 92 93 94 95 96 97 98
%Plot norm of control commands
subplot(1,2,1); hold on;
for n=1:nbRepros
	plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
end
xlabel('t'); ylabel('|ddx|');
%Plot stiffness
subplot(1,2,2); hold on;
for n=1:nbRepros
	plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
end
xlabel('t'); ylabel('kp');

Sylvain Calinon's avatar
Sylvain Calinon committed
99
%print('-dpng','graphs/demo_testLQR01.png');
Milad Malekzadeh's avatar
Milad Malekzadeh committed
100 101 102
%pause;
%close all;