reproduction_LQR_finiteHorizon.m 3.72 KB
Newer Older
Sylvain Calinon's avatar
Sylvain Calinon committed
1
function r = reproduction_LQR_finiteHorizon(model, r, currPos, rFactor, Pfinal)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
2 3 4 5 6
% Reproduction with a linear quadratic regulator of finite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
%          http://programming-by-demonstration.org
%
Milad Malekzadeh's avatar
Milad Malekzadeh committed
7 8
% This source code is given for free! In exchange, we 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
%
% @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"
% }

Sylvain Calinon's avatar
Sylvain Calinon committed
20
[nbVarOut,nbData] = size(r.currTar);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
21 22 23 24 25 26 27 28 29

%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) (See Eq. (5.0.2) in doc/TechnicalReport.pdf)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Definition of a double integrator system (DX = A X + B u with X = [x; dx])
A = kron([0 1; 0 0], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf
B = kron([0; 1], eye(nbVarOut)); %See Eq. (5.1.1) in doc/TechnicalReport.pdf
%Initialize Q and R weighting matrices
Q = zeros(nbVarOut*2,nbVarOut*2,nbData);
for t=1:nbData
Milad Malekzadeh's avatar
Milad Malekzadeh committed
30
	Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
31 32 33 34 35 36 37 38 39 40 41 42 43 44
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
45 46
%Compute P_T from the desired final feedback gains L_T,
P(:,:,nbData) = Pfinal;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
47 48

%Variables for feedforward term computation (optional for movements with low dynamics)
Sylvain Calinon's avatar
Sylvain Calinon committed
49
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
Milad Malekzadeh's avatar
Milad Malekzadeh committed
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68
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
	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)); %See Eq. (5.1.11) in doc/TechnicalReport.pdf
	%Optional feedforward term computation
	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)); %See Eq. (5.1.29) in doc/TechnicalReport.pdf
end
%Computation of the feedback term L and feedforward term M in u=-LX+M
for t=1:nbData
	L(:,:,t) = R\B' * P(:,:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
	M(:,t) = R\B' * d(:,t); %Optional feedforward term computation (See Eq. (5.1.30) in doc/TechnicalReport.pdf)
end

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

Milad Malekzadeh's avatar
Milad Malekzadeh committed
70 71
for t=1:nbData
	%Compute acceleration (with only feedback term)
Sylvain Calinon's avatar
Sylvain Calinon committed
72
	%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
Milad Malekzadeh's avatar
Milad Malekzadeh committed
73
	
Milad Malekzadeh's avatar
Milad Malekzadeh committed
74
	%Compute acceleration (with both feedback and feedforward terms)
Sylvain Calinon's avatar
Sylvain Calinon committed
75
	ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
Milad Malekzadeh's avatar
Milad Malekzadeh committed
76
	
Milad Malekzadeh's avatar
Milad Malekzadeh committed
77 78 79
	%Update velocity and position
	dx = dx + ddx * model.dt;
	x = x + dx * model.dt;
80 81

	%Log data (with additional variables collected for analysis purpose)
Sylvain Calinon's avatar
Sylvain Calinon committed
82
	r.Data(:,t) = x;
Milad Malekzadeh's avatar
Milad Malekzadeh committed
83
	r.ddxNorm(t) = norm(ddx);
84 85 86 87
	%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
88
	r.kpDet(t) = det(L(:,1:nbVarOut,t));
89 90
	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
91 92
end