reproduction_LQR_infiniteHorizon.m 3.54 KB
Newer Older
Milad Malekzadeh's avatar
Milad Malekzadeh committed
1
function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
2
% Reproduction with a linear quadratic regulator of infinite horizon
Milad Malekzadeh's avatar
Milad Malekzadeh committed
3 4 5 6
%
% 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 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"
% }

nbData = size(DataIn,2);
nbVarOut = model.nbVar - size(DataIn,1);

Milad Malekzadeh's avatar
Milad Malekzadeh committed
23
%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
24 25 26 27 28 29 30 31 32 33 34 35 36 37
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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);
R = eye(nbVarOut) * rFactor;

%Variables for feedforward term computation (optional for movements with low dynamics)
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
%dtar = gradient(tar,1,2)/model.dt;
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;

Milad Malekzadeh's avatar
Milad Malekzadeh committed
38

Milad Malekzadeh's avatar
Milad Malekzadeh committed
39 40 41 42 43 44
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
	%Current weighting term
Milad Malekzadeh's avatar
Milad Malekzadeh committed
45
	Q(1:nbVarOut,1:nbVarOut) = inv(r.currSigma(:,:,t));
Milad Malekzadeh's avatar
Milad Malekzadeh committed
46
	
Milad Malekzadeh's avatar
Milad Malekzadeh committed
47
	%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
Milad Malekzadeh's avatar
Milad Malekzadeh committed
48
	%also available with the control toolbox of GNU Octave
Milad Malekzadeh's avatar
Milad Malekzadeh committed
49
	%P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
Milad Malekzadeh's avatar
Milad Malekzadeh committed
50 51 52
	
	%Alternatively, the function below can be used for an implementation based on Schur decomposition
	%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
53 54
	
	%Alternatively, the function below can be used for an implementation based on Eigendecomposition
55
	%-> the only operator is eig([A -B/R*B'; -Q -A'])
Milad Malekzadeh's avatar
Milad Malekzadeh committed
56
	P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2); %See Eq. (5.1.27) and Sec. (5.2) in doc/TechnicalReport.pdf
Milad Malekzadeh's avatar
Milad Malekzadeh committed
57 58
	
	%Variable for feedforward term computation (optional for movements with low dynamics)
59
	d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %See Eq. (5.1.28) with d_dot=0 in doc/TechnicalReport.pdf
Milad Malekzadeh's avatar
Milad Malekzadeh committed
60 61 62
	
	%Feedback term
	L = R\B'*P; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
63
	
Milad Malekzadeh's avatar
Milad Malekzadeh committed
64 65 66 67 68 69 70 71 72 73 74 75
	%Feedforward term
	M = R\B'*d; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
	
	%Compute acceleration (with only feedback terms)
	%ddx =  -L * [x-r.currTar(:,t); dx];
	
	%Compute acceleration (with feedback and feedforward terms)
	ddx =  -L * [x-r.currTar(:,t); dx] + M; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
	
	%Update velocity and position
	dx = dx + ddx * model.dt;
	x = x + dx * model.dt;
76 77
	
	%Log data (with additional variables collected for analysis purpose)
Milad Malekzadeh's avatar
Milad Malekzadeh committed
78
	r.Data(:,t) = [DataIn(:,t); x];
Milad Malekzadeh's avatar
Milad Malekzadeh committed
79
	r.ddxNorm(t) = norm(ddx);
80 81 82 83
	%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
	%r.FF(:,t) = M;
	%r.Kp(:,:,t) = L(:,1:nbVarOut);
	%r.Kv(:,:,t) = L(:,nbVarOut+1:end);
Milad Malekzadeh's avatar
Milad Malekzadeh committed
84 85
	r.kpDet(t) = det(L(:,1:nbVarOut));
	r.kvDet(t) = det(L(:,nbVarOut+1:end));
86
	%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
87 88 89
end