reproduction_LQR_finiteHorizon.m 4.39 KB
Newer Older
Sylvain Calinon's avatar
Sylvain Calinon committed
1
function r = reproduction_LQR_finiteHorizon(model, r, currPos, rFactor, Pfinal)
2
% Reproduction with a linear quadratic regulator of finite horizon (continuous version).
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 10 11 12 13 14 15 16 17
%
% @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"
% }
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
%
% @article{Calinon15,
%   author="Calinon, S.",
%   title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
%   journal="Intelligent Service Robotics",
%   year="2015"
% }
%
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon (http://calinon.ch/), 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
43

Sylvain Calinon's avatar
Sylvain Calinon committed
44
[nbVarOut,nbData] = size(r.currTar);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
45

46
%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) 
Milad Malekzadeh's avatar
Milad Malekzadeh committed
47 48
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Definition of a double integrator system (DX = A X + B u with X = [x; dx])
49 50
A = kron([0 1; 0 0], eye(nbVarOut)); 
B = kron([0; 1], eye(nbVarOut)); 
Milad Malekzadeh's avatar
Milad Malekzadeh committed
51 52 53
%Initialize Q and R weighting matrices
Q = zeros(nbVarOut*2,nbVarOut*2,nbData);
for t=1:nbData
Milad Malekzadeh's avatar
Milad Malekzadeh committed
54
	Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
55 56 57 58 59 60 61 62 63 64 65 66 67 68
end
R = eye(nbVarOut) * rFactor;

if nargin<6
	%Pfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
	Pfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions)
end

%Auxiliary variables to minimize the cost function
P = zeros(nbVarOut*2, nbVarOut*2, nbData);
d = zeros(nbVarOut*2, nbData); %For optional feedforward term computation

%Feedback term
L = zeros(nbVarOut, nbVarOut*2, nbData);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
69 70
%Compute P_T from the desired final feedback gains L_T,
P(:,:,nbData) = Pfinal;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
71 72

%Variables for feedforward term computation (optional for movements with low dynamics)
Sylvain Calinon's avatar
Sylvain Calinon committed
73
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
Milad Malekzadeh's avatar
Milad Malekzadeh committed
74 75 76 77 78
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;

%Backward integration of the Riccati equation and additional equation
for t=nbData-1:-1:1
79
	P(:,:,t) = P(:,:,t+1) + model.dt * (A'*P(:,:,t+1) + P(:,:,t+1)*A - P(:,:,t+1)*B*(R\B')*P(:,:,t+1) + Q(:,:,t+1));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
80
	%Optional feedforward term computation
81
	d(:,t) = d(:,t+1) + model.dt * ((A'-P(:,:,t+1)*B*(R\B'))*d(:,t+1) + P(:,:,t+1)*dtar(:,t+1) - P(:,:,t+1)*A*tar(:,t+1)); 
Milad Malekzadeh's avatar
Milad Malekzadeh committed
82 83 84
end
%Computation of the feedback term L and feedforward term M in u=-LX+M
for t=1:nbData
85 86
	L(:,:,t) = R\B' * P(:,:,t); %feedback term
	M(:,t) = R\B' * d(:,t); %feedforward term  
Milad Malekzadeh's avatar
Milad Malekzadeh committed
87 88 89 90 91 92
end

%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
Sylvain Calinon's avatar
Sylvain Calinon committed
93

Milad Malekzadeh's avatar
Milad Malekzadeh committed
94 95
for t=1:nbData
	%Compute acceleration (with only feedback term)
Sylvain Calinon's avatar
Sylvain Calinon committed
96
	%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
Milad Malekzadeh's avatar
Milad Malekzadeh committed
97
	
Milad Malekzadeh's avatar
Milad Malekzadeh committed
98
	%Compute acceleration (with both feedback and feedforward terms)
99
	ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); 
Milad Malekzadeh's avatar
Milad Malekzadeh committed
100
	
Milad Malekzadeh's avatar
Milad Malekzadeh committed
101 102 103
	%Update velocity and position
	dx = dx + ddx * model.dt;
	x = x + dx * model.dt;
104 105

	%Log data (with additional variables collected for analysis purpose)
Sylvain Calinon's avatar
Sylvain Calinon committed
106
	r.Data(:,t) = x;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
107
	r.ddxNorm(t) = norm(ddx);
108 109 110 111
	%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
	%r.FF(:,t) = M(:,t);
	%r.Kp(:,:,t) = L(:,1:nbVarOut,t);
	%r.Kv(:,:,t) = L(:,nbVarOut+1:end,t);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
112
	r.kpDet(t) = det(L(:,1:nbVarOut,t));
113 114
	r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
	%Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V'
Milad Malekzadeh's avatar
Milad Malekzadeh committed
115
end