demo_testLQR01.m 4.39 KB
Newer Older
Milad Malekzadeh's avatar
Milad Malekzadeh committed
1
2
3
4
5
6
function demo_testLQR01
% Test of the linear quadratic regulation
%
% Author:	Sylvain Calinon, 2014
%         http://programming-by-demonstration.org/SylvainCalinon
%
Milad Malekzadeh's avatar
Tab2.    
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
10
11
12
13
14
15
16
17
18
19
20
21
22
%
% @inproceedings{Calinon14ICRA,
%   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"
% }

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

Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
28
%% Reproduction with LQR
Milad Malekzadeh's avatar
Milad Malekzadeh committed
29
30
31
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
32
a.currTar = ones(1,nbData);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
for n=1:nbRepros
	a.currSigma = ones(1,1,nbData) * 10^(2-n);
	%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
	r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
end

%% 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
Tab2.    
Milad Malekzadeh committed
51
figure;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
%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');

%pause;
%close all;
function demo_testLQR01
% Test of the linear quadratic regulation
%
% Author:	Sylvain Calinon, 2014
%         http://programming-by-demonstration.org/SylvainCalinon
%
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
73
74
% 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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
%
% @inproceedings{Calinon14ICRA,
%   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"
% }

%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
89
model.dt = 0.01; %Time step
Milad Malekzadeh's avatar
Milad Malekzadeh committed
90
91
92
93
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

Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
94
%% Reproduction with LQR
Milad Malekzadeh's avatar
Milad Malekzadeh committed
95
96
97
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
98
a.currTar = ones(1,nbData);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
99
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
100
101
102
	a.currSigma = ones(1,1,nbData) * 10^(2-n);
	%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
	r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
103
104
105
106
107
108
109
110
111
end

%% 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
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
112
113
	%Plot trajectories
	plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
114
115
116
end
xlabel('t'); ylabel('x_1');

Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
117
figure;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
118
119
120
%Plot norm of control commands
subplot(1,2,1); hold on;
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
121
	plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
122
123
124
125
126
end
xlabel('t'); ylabel('|ddx|');
%Plot stiffness
subplot(1,2,2); hold on;
for n=1:nbRepros
Milad Malekzadeh's avatar
Tab2.    
Milad Malekzadeh committed
127
	plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
128
129
130
131
132
end
xlabel('t'); ylabel('kp');

%pause;
%close all;