demo_testLQR02.m 4.82 KB
Newer Older
Milad Malekzadeh's avatar
Milad Malekzadeh committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148
function demo_testLQR02
% Test of the linear quadratic regulation
%
% Author:	Sylvain Calinon, 2014
%         http://programming-by-demonstration.org/SylvainCalinon
%
% 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: 
%
% @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)
model.dt = 0.01; %Time step 
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

%% Reproduction with LQR 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData);
a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u 
for n=1:nbRepros
	%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,:), 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('x_1');

figure; 
%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
	plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('kp/kv');

r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071

%pause;
%close all;
function demo_testLQR02
% Test of the linear quadratic regulation
%
% Author:	Sylvain Calinon, 2014
%         http://programming-by-demonstration.org/SylvainCalinon
%
% 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: 
%
% @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)
model.dt = 0.01; %Time step 
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

%% Reproduction with LQR 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Reproductions with LQR...');
DataIn = [1:nbData] * model.dt;
a.currTar = ones(1,nbData);
a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u 
for n=1:nbRepros
  %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,:), 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('x_1');

figure; 
%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
  plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2);
end
xlabel('t'); ylabel('kp/kv');

r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071

%pause;
%close all;