demo_testLQR02.m 3.6 KB
Newer Older
Milad Malekzadeh's avatar
Milad Malekzadeh committed
1
function demo_testLQR02
2
% Test of the linear quadratic regulation (evaluation of the damping ratio found by the system).
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
%
Sylvain Calinon's avatar
Sylvain Calinon committed
9 10
% @article{Calinon15,
%   author="Calinon, S.",
11 12 13 14 15 16 17 18 19 20 21 22 23
%   title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
%   journal="Intelligent Service Robotics",
%   year="2015"
% }
%
% @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
24
% }
25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
% 
% 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
42

43 44
addpath('./m_fcts/');

Sylvain Calinon's avatar
Sylvain Calinon committed
45

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

Sylvain Calinon's avatar
Sylvain Calinon committed
54

Milad Malekzadeh's avatar
Milad Malekzadeh committed
55
%% Reproduction with LQR
Milad Malekzadeh's avatar
Milad Malekzadeh committed
56 57 58 59
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData);
60
a.currSigma = ones(1,1,nbData); %-> LQR with cost X'X + u'u
Milad Malekzadeh's avatar
Milad Malekzadeh committed
61
for n=1:nbRepros
Sylvain Calinon's avatar
Sylvain Calinon committed
62 63 64
	%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
65 66
end

Sylvain Calinon's avatar
Sylvain Calinon committed
67

Milad Malekzadeh's avatar
Milad Malekzadeh committed
68 69 70
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
71
hold on; box on;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
72 73 74 75 76 77 78 79
%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,:), 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('x_1');

Milad Malekzadeh's avatar
Milad Malekzadeh committed
80
figure;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
81 82 83 84 85 86 87 88 89 90 91 92 93 94 95
%Plot norm of control commands
subplot(1,3,1); hold on;
for n=1:nbRepros
	plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('|ddx|');
%Plot stiffness
subplot(1,3,2); hold on;
for n=1:nbRepros
	plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('kp');
%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5)
subplot(1,3,3); hold on;
for n=1:nbRepros
96 97 98
	%Ideal damping ratio of 1/2^.5 = 0.7071, corresponding to r(n).kvDet(1) = (2*r(n).kpDet(1))^.5
	dampingRatio = r(n).kvDet(:) ./ (2*r(n).kpDet(:).^.5);
	plot(DataIn, dampingRatio, 'k-', 'linewidth', 2);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
99
end
100
xlabel('t'); ylabel('Damping ratio');
Milad Malekzadeh's avatar
Milad Malekzadeh committed
101

Sylvain Calinon's avatar
Sylvain Calinon committed
102
%print('-dpng','graphs/demo_testLQR02.png');
103 104
%pause;
%close all;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
105