demo_testLQR02.m 2.37 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
%
Sylvain Calinon's avatar
Sylvain Calinon committed
4 5
% Sylvain Calinon, 2015
% http://programming-by-demonstration.org/lib/
Milad Malekzadeh's avatar
Milad Malekzadeh committed
6
%
Milad Malekzadeh's avatar
Milad Malekzadeh committed
7 8
% 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:
Milad Malekzadeh's avatar
Milad Malekzadeh committed
9
%
Sylvain Calinon's avatar
Sylvain Calinon committed
10 11 12 13
% @article{Calinon15,
%   author="Calinon, S.",
%   title="A tutorial on task-parameterized movement learning and retrieval",
%   year="2015",
Milad Malekzadeh's avatar
Milad Malekzadeh committed
14 15
% }

16 17
addpath('./m_fcts/');

Sylvain Calinon's avatar
Sylvain Calinon committed
18

Milad Malekzadeh's avatar
Milad Malekzadeh committed
19 20 21
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
22
model.dt = 0.01; %Time step
Milad Malekzadeh's avatar
Milad Malekzadeh committed
23 24 25 26
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
27

Milad Malekzadeh's avatar
Milad Malekzadeh committed
28
%% Reproduction with LQR
Milad Malekzadeh's avatar
Milad Malekzadeh committed
29 30 31 32
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData);
33
a.currSigma = ones(1,1,nbData); %-> LQR with cost X'X + u'u
Milad Malekzadeh's avatar
Milad Malekzadeh committed
34
for n=1:nbRepros
Sylvain Calinon's avatar
Sylvain Calinon committed
35 36 37
	%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
38 39
end

Sylvain Calinon's avatar
Sylvain Calinon committed
40

Milad Malekzadeh's avatar
Milad Malekzadeh committed
41 42 43
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,50,1300,500]);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
44
hold on; box on;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
45 46 47 48 49 50 51 52
%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
53
figure;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68
%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
69 70 71
	%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
72
end
73
xlabel('t'); ylabel('Damping ratio');
Milad Malekzadeh's avatar
Milad Malekzadeh committed
74

Sylvain Calinon's avatar
Sylvain Calinon committed
75
%print('-dpng','graphs/demo_testLQR02.png');
76 77
%pause;
%close all;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
78