Commit 43f2e613 authored by Sylvain Calinon's avatar Sylvain Calinon

Notational changes in LQR files and update of kmeans init

parent edcab51a
......@@ -44,9 +44,9 @@ for n=1:nbRepros
r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
else
%First call to LQR to get an estimate of the final feedback terms
[~,Sfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
[~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
%Second call to LQR with finite horizon
r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Sfinal);
r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
end
end
for n=1:nbRepros
......
function model = init_tensorGMM_kmeans(Data, model)
% Author: Leonel Rozo, 2014
% http://programming-by-demonstration.org/LeonelRozo
%
% Initialization of the model with k-means.
% Authors: Sylvain Calinon, Tohid Alizadeh, 2013
% http://programming-by-demonstration.org/
diagRegularizationFactor = 1E-4;
%Matricization/flattening of tensor
DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3));
DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
%The function 'kmeans' below is from the Matlab Statistics Toolbox (see note above)
[Data_id, Centers] = kmeans(DataAll', model.nbStates);
%k-means clustering
[Data_id, Mu] = kmeansClustering(DataAll, model.nbStates);
% Setting means and covariance matrices
Mu = Centers';
Sigma = zeros(model.nbFrames*model.nbVar, model.nbFrames*model.nbVar, model.nbStates);
for i = 1 : model.nbStates
for i=1:model.nbStates
idtmp = find(Data_id==i);
model.Priors(i) = length(idtmp);
Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor;
Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor;
end
model.Priors = model.Priors / sum(model.Priors);
%Reshape GMM parameters into a tensor
for m = 1 : model.nbFrames
for i = 1 : model.nbStates
model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i);
model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
%Reshape GMM parameters into a tensor
for m=1:model.nbFrames
for i=1:model.nbStates
model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i);
model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
end
end
function [idList, Mu] = kmeansClustering(Data, nbStates)
% Initialization of the model with k-means.
% Author: Sylvain Calinon, Tohid Alizadeh, 2013
% Authors: Sylvain Calinon, Tohid Alizadeh, 2013
% http://programming-by-demonstration.org/
%Criterion to stop the EM iterative update
......
function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Sfinal)
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
......@@ -28,23 +28,23 @@ 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));
Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t));
end
R = eye(nbVarOut) * rFactor;
if nargin<6
%Sfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
Sfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions)
%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
S = zeros(nbVarOut*2, nbVarOut*2, nbData);
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 S_T from the desired final feedback gains L_T,
S(:,:,nbData) = Sfinal;
%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
......@@ -62,14 +62,14 @@ dtar = gradient(tar,1,2)/model.dt;
%Backward integration of the Riccati equation and additional equation
for t=nbData-1:-1:1
S(:,:,t) = S(:,:,t+1) + model.dt * (A'*S(:,:,t+1) + S(:,:,t+1)*A - S(:,:,t+1) * B * (R\B') * S(:,:,t+1) + Q(:,:,t+1));
%Optional feedforward term computation
d(:,t) = d(:,t+1) + model.dt * ((A'-S(:,:,t+1)*B*(R\B'))*d(:,t+1) + S(:,:,t+1)*dtar(:,t+1) - S(:,:,t+1)*A*tar(:,t+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 in u=-LX+M
%Computation of the feedback term L and feedforward term M in u=-LX+M
for t=1:nbData
L(:,:,t) = R\B' * S(:,:,t);
M(:,t) = R\B' * d(:,t); %Optional feedforward term computation
L(:,:,t) = R\B' * P(:,:,t);
M(:,t) = R\B' * d(:,t); %Optional feedforward term computation
end
%% Reproduction with varying impedance parameters
......@@ -77,26 +77,22 @@ end
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);
%ddx = -L * ([x;dx]-tar(:,t)) + M;
%r.FB(:,t) = -L * ([x;dx]-tar(:,t));
%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,t));
r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
%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,S] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
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
......@@ -32,14 +32,9 @@ 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;
%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
%dtar = diff([tar tar(:,1)],1,2)/model.dt;
%% Reproduction with varying impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
......@@ -51,16 +46,16 @@ for t=1:nbData
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
S = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
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.
%S = solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
%P = solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
%Variable for feedforward term computation (optional for movements with low dynamics)
d = inv(S*B*(R\B')-A') * (S*dtar(:,t) - S*A*tar(:,t));
d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t));
%Feedback term
L = R\B'*S;
L = R\B'*P;
%Feedforward term
M = R\B'*d;
......@@ -69,11 +64,7 @@ for t=1:nbData
%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;
%ddx = -L * ([x;dx]-tar(:,t)) + M;
%r.FB(:,t) = -L * ([x;dx]-tar(:,t));
%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M;
%Update velocity and position
......
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