Skip to content

pbdlib-matlab
pbdlib-matlab
Commit 68cb8b63 authored by Milad Malekzadeh's avatar Milad Malekzadeh
Browse files
Options

Comments added.

parent 10deffa2
Hide whitespace changes
Inline Side-by-side
Showing with 188 additions and 188 deletions
reproduction_LQR_finiteHorizon.m
View file @ 68cb8b63
function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal)
% Reproduction with a linear quadratic regulator of finite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
% http://programming-by-demonstration.org
%
% 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:
%
% @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);
%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Definition of a double integrator system (DX = A X + B u with X = [x; dx])
A = kron([0 1; 0 0], eye(nbVarOut));
B = kron([0; 1], eye(nbVarOut));
%Initialize Q and R weighting matrices
Q = zeros(nbVarOut*2,nbVarOut*2,nbData);
for t=1:nbData
Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
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);
%Compute P_T from the desired final feedback gains L_T,
P(:,:,nbData) = Pfinal;
%Compute derivative of target path
%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation
%Variables for feedforward term computation (optional for movements with low dynamics)
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
%dtar = gradient(tar,1,2)/model.dt;
%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
%dtar = diff([tar tar(:,1)],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));
%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));
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);
M(:,t) = R\B' * d(:,t); %Optional feedforward term computation
end
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
%Compute acceleration (with only feedback term)
%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
%Compute acceleration (with both feedback and feedforward terms)
ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t);
r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
r.FF(:,t) = M(:,t);
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut,t));
r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
end
function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal)
% Reproduction with a linear quadratic regulator of finite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
% http://programming-by-demonstration.org
%
% 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:
%
% @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);
%% 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
Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
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);
%Compute P_T from the desired final feedback gains L_T,
P(:,:,nbData) = Pfinal;
%Compute derivative of target path
%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation
%Variables for feedforward term computation (optional for movements with low dynamics)
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
%dtar = gradient(tar,1,2)/model.dt;
%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
%dtar = diff([tar tar(:,1)],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);
for t=1:nbData
%Compute acceleration (with only feedback term)
%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
%Compute acceleration (with both feedback and feedforward terms)
ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
r.FF(:,t) = M(:,t);
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut,t));
r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
end
reproduction_LQR_infiniteHorizon.m
View file @ 68cb8b63
function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
% Reproduction with a linear quadratic regulator of infinite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
% http://programming-by-demonstration.org
%
% 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:
%
% @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);
%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Definition of a double integrator system (DX = A X + B u with X = [x; dx])
A = kron([0 1; 0 0], eye(nbVarOut));
B = kron([0; 1], eye(nbVarOut));
%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;
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
%Current weighting term
Q(1:nbVarOut,1:nbVarOut) = inv(r.currSigma(:,:,t));
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
%P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
%Alternatively, the function below can be used for an implementation based on Schur decomposition
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%Alternatively, the function below can be used for an implementation based on Eigendecomposition
P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2);
%Variable for feedforward term computation (optional for movements with low dynamics)
d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t));
%Feedback term
L = R\B'*P;
%Feedforward term
M = R\B'*d;
%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;
r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
r.FF(:,t) = M;
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut));
r.kvDet(t) = det(L(:,nbVarOut+1:end));
end
function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
% Reproduction with a linear quadratic regulator of infinite horizon
%
% Authors: Sylvain Calinon and Danilo Bruno, 2014
% http://programming-by-demonstration.org
%
% 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:
%
% @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);
%% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%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;
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
%Current weighting term
Q(1:nbVarOut,1:nbVarOut) = inv(r.currSigma(:,:,t));
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
%P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
%Alternatively, the function below can be used for an implementation based on Schur decomposition
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
%Alternatively, the function below can be used for an implementation based on Eigendecomposition
P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2); %See Eq. (5.1.27) and Sec. (5.2) in doc/TechnicalReport.pdf
%Variable for feedforward term computation (optional for movements with low dynamics)
d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t)); %See Eq. (5.1.28) in doc/TechnicalReport.pdf
%Feedback term
L = R\B'*P; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
%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
r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
r.FF(:,t) = M;
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
r.kpDet(t) = det(L(:,1:nbVarOut));
r.kvDet(t) = det(L(:,nbVarOut+1:end));
end
solveAlgebraicRiccati_Schur.m
View file @ 68cb8b63
......@@ -11,8 +11,8 @@ function X = solveAlgebraicRiccati_Schur(A, G, Q)
%}
n = size(A,1);
Z = [A -G; -Q -A'];
Z = [A -G; -Q -A']; %See Eq. (5.2.3) in doc/TechnicalReport.pdf
[U0, S0] = schur(Z);
U = ordschur(U0, S0, 'lhp'); %Not yet available in GNU Octave
X = U(n+1:end,1:n) / U(1:n,1:n);
[U0, S0] = schur(Z); %See Eq. (5.2.6) in doc/TechnicalReport.pdf
U = ordschur(U0, S0, 'lhp'); %Not yet available in GNU Octave (See Eq. (5.2.6) in doc/TechnicalReport.pdf)
X = U(n+1:end,1:n) / U(1:n,1:n); %See Eq. (5.2.7) in doc/TechnicalReport.pdf
solveAlgebraicRiccati_eig.m
View file @ 68cb8b63
......@@ -3,9 +3,9 @@ function X = solveAlgebraicRiccati_eig(A, G, Q)
%Danilo Bruno, 2014
n = size(A,1);
Z = [A -G; -Q -A'];
Z = [A -G; -Q -A']; %See Eq. (5.2.3) in doc/TechnicalReport.pdf
[V,D] = eig(Z);
[V,D] = eig(Z); %See Eq. (5.2.4) in doc/TechnicalReport.pdf
U = [];
for j=1:2*n
if real(D(j,j)) < 0
......@@ -13,5 +13,5 @@ for j=1:2*n
end
end
X = U(n+1:end,1:n) / U(1:n,1:n);
X = U(n+1:end,1:n) / U(1:n,1:n); %See Eq. (5.2.5) in doc/TechnicalReport.pdf
X = real(X);
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment